diff --git a/include/laser/processor/processor_closeloop_icp.h b/include/laser/processor/processor_closeloop_icp.h index b0a829d720291e8a8f0e96d0d14fae78c743e5a5..e06f241b84df4f398924c3f40c3e34202932f2c3 100644 --- a/include/laser/processor/processor_closeloop_icp.h +++ b/include/laser/processor/processor_closeloop_icp.h @@ -21,7 +21,7 @@ struct ProcessorParamsCloseLoopICP : public ProcessorParamsBase { int match_past_key_frame; ProcessorParamsCloseLoopICP() = default; - ProcessorParamsCloseLoopICP(std::string _unique_name, const paramsServer& _server): + ProcessorParamsCloseLoopICP(std::string _unique_name, const ParamsServer& _server): ProcessorParamsBase(_unique_name, _server) { match_past_key_frame = _server.getParam<int>(_unique_name + "/match_past_key_frame"); @@ -47,7 +47,7 @@ class ProcessorCloseLoopICP : public ProcessorBase virtual bool voteForKeyFrame() override; - static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr); + static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr); static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr); diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h index 56da0c09150ff9927cc0d706ebdcb6d49b53ad0b..a17429b0986630273689ffa8897b124799aae5db 100644 --- a/include/laser/processor/processor_odom_icp.h +++ b/include/laser/processor/processor_odom_icp.h @@ -34,7 +34,7 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker int vfk_max_points; ProcessorParamsOdomICP() = default; - ProcessorParamsOdomICP(std::string _unique_name, const paramsServer &_server): + ProcessorParamsOdomICP(std::string _unique_name, const ParamsServer &_server): ProcessorParamsTracker(_unique_name, _server) { use_point_to_line_distance = _server.getParam<int>(_unique_name + "/use_point_to_line_distance"); @@ -110,7 +110,7 @@ class ProcessorOdomICP : public ProcessorTracker virtual void configure(SensorBasePtr _sensor) override; static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr); - static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr); + static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr); diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h index 9d4b448a422d294a5132bba917bd0abbbd2caec3..e94731ea71a0dfa712864a7dc9d9c8508e3661c1 100644 --- a/include/laser/processor/processor_tracker_feature_polyline_2D.h +++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h @@ -9,7 +9,6 @@ #define PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_ #include "laser/processor/polyline_2D_utils.h" -#include "core/processor/processor_tracker_feature.h" #include "laser/landmark/landmark_polyline_2D.h" #include "laser/landmark/landmark_match_polyline_2D.h" #include "laser/sensor/sensor_laser_2D.h" @@ -19,6 +18,9 @@ #include "laser/factor/factor_point_2D.h" #include "laser/factor/factor_point_to_line_2D.h" +#include "core/processor/processor_tracker_feature.h" +#include "core/utils/params_server.hpp" + //laser_scan_utils #include "laser_scan_utils/laser_scan.h" #include "laser_scan_utils/line_finder_iterative.h" @@ -234,6 +236,10 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr); + static ProcessorBasePtr createAutoConf(const std::string& _unique_name, + const ParamsServer& _server, + const SensorBasePtr _sensor_ptr); + const FeatureBasePtrList& getLastNewFeatures() const { return untracked_features_last_; diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h index 64caa44e3fd96f374c90b08faa7f1b0336a3c218..200f00badcd386b2ea2d6945f6d244e71b993b1a 100644 --- a/include/laser/sensor/sensor_laser_2D.h +++ b/include/laser/sensor/sensor_laser_2D.h @@ -77,7 +77,7 @@ class SensorLaser2D : public SensorBase 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 SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server); static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml); }; diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp index c2815122e4db9806ec4e83d66a39d19feacd886d..d4f410071df0de2110f27ad7a4878c8ee46ab519 100644 --- a/src/processor/processor_closeloop_icp.cpp +++ b/src/processor/processor_closeloop_icp.cpp @@ -56,7 +56,7 @@ ProcessorBasePtr ProcessorCloseLoopICP::create(const std::string& _unique_name, return prc_ptr; } -ProcessorBasePtr ProcessorCloseLoopICP::createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr) +ProcessorBasePtr ProcessorCloseLoopICP::createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr) { ProcessorCloseLoopICPPtr prc_ptr; auto params = std::make_shared<ProcessorParamsCloseLoopICP>(); diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp index e96debd03fcf2bcbf6f2453f7c25e9fe50989421..4bbf3903b51f348927cb143eec8c30a3745b514f 100644 --- a/src/processor/processor_odom_icp.cpp +++ b/src/processor/processor_odom_icp.cpp @@ -188,7 +188,7 @@ ProcessorBasePtr ProcessorOdomICP::create(const std::string& _unique_name, const return prc_ptr; } -ProcessorBasePtr ProcessorOdomICP::createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr) +ProcessorBasePtr ProcessorOdomICP::createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr) { ProcessorOdomICPPtr prc_ptr; diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index ed3a2af962a180d57f4a0c884abfb801aabd4f81..9c877522e15a4a25e3ad89a9e0e6d661e693edfd 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -1299,6 +1299,12 @@ ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::create(const std::string& _u return prc_ptr; } +ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::createAutoConf(const std::string& _unique_name, + const ParamsServer& _server, + const SensorBasePtr _sensor_ptr) +{ + return nullptr; +} } /* namespace wolf */ // Register in the SensorFactory @@ -1306,3 +1312,7 @@ ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::create(const std::string& _u namespace wolf { WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D) } /* namespace wolf */ +#include "core/processor/autoconf_processor_factory.h" +namespace wolf { + WOLF_REGISTER_PROCESSOR_AUTO("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D) +} // namespace wolf \ No newline at end of file diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp index f1ddf33abe02f9d732a62e6182f7dcec8d1b1dbc..ce551eb2d29a2ad5baf0195e1cbbeb04ecfc9500 100644 --- a/src/sensor/sensor_laser_2D.cpp +++ b/src/sensor/sensor_laser_2D.cpp @@ -79,7 +79,7 @@ SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen return sen; } -SensorBasePtr SensorLaser2D::createAutoConf(const std::string& _unique_name, const paramsServer& _server) +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]");