diff --git a/CMakeLists.txt b/CMakeLists.txt index fb83ccf5399baef48a2267f0657cf68d8bbd60db..7dafd62f1dcd36919049428014466507f73cb70e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,10 +185,10 @@ SET(HDRS_UTILS include/core/utils/converter.h include/core/utils/eigen_assert.h include/core/utils/eigen_predicates.h - include/core/utils/loader.hpp + include/core/utils/loader.h include/core/utils/logging.h include/core/utils/make_unique.h - include/core/utils/params_server.hpp + include/core/utils/params_server.h include/core/utils/singleton.h include/core/utils/utils_gtest.h include/core/utils/converter_utils.h @@ -287,7 +287,7 @@ SET(HDRS_SOLVER ) SET(HDRS_YAML - include/core/yaml/parser_yaml.hpp + include/core/yaml/parser_yaml.h include/core/yaml/yaml_conversion.h ) #SOURCES @@ -321,6 +321,8 @@ SET(SRCS_MATH ) SET(SRCS_UTILS src/utils/converter_utils.cpp + src/utils/params_server.cpp + src/utils/loader.cpp ) SET(SRCS_CAPTURE @@ -369,6 +371,7 @@ SET(SRCS_SOLVER src/solver/solver_manager.cpp ) SET(SRCS_YAML + src/yaml/parser_yaml.cpp src/yaml/processor_odom_3d_yaml.cpp src/yaml/sensor_odom_2d_yaml.cpp src/yaml/sensor_odom_3d_yaml.cpp diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp index a97d8d2f9c5c7fb8c7f1f37c12330038beb3ba98..a387adfe9656523c37d025bb3a74f0f198281b3d 100644 --- a/hello_wolf/hello_wolf_autoconf.cpp +++ b/hello_wolf/hello_wolf_autoconf.cpp @@ -9,7 +9,7 @@ // wolf core includes #include "core/common/wolf.h" #include "core/capture/capture_odom_2d.h" -#include "core/yaml/parser_yaml.hpp" +#include "core/yaml/parser_yaml.h" #include "core/ceres_wrapper/ceres_manager.h" // hello wolf local includes diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h index e62bfc1a5cd4ff7115cac06f9738204c71017f2b..491efff4f533edc993051482de7d0e84562a790a 100644 --- a/hello_wolf/sensor_range_bearing.h +++ b/hello_wolf/sensor_range_bearing.h @@ -9,7 +9,7 @@ #define HELLO_WOLF_SENSOR_RANGE_BEARING_H_ #include "core/sensor/sensor_base.h" -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" namespace wolf { diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index a5eac842ef7afa410d15a42e7e749ddccc97635c..2491d5a6a5afbdc4d3b52a3e86003c3794b6cd81 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -103,7 +103,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all); protected: - SizeEigen computeCalibSize() const; + virtual SizeEigen computeCalibSize() const; private: FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr); diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h index f124325950a06f74642cff38b6d0c053958cab8e..802ab7b49dabf93455f8f93fc8c30745415fe33e 100644 --- a/include/core/ceres_wrapper/ceres_manager.h +++ b/include/core/ceres_wrapper/ceres_manager.h @@ -8,7 +8,7 @@ //wolf includes #include "core/solver/solver_manager.h" -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" #include "core/ceres_wrapper/cost_function_wrapper.h" #include "core/ceres_wrapper/local_parametrization_wrapper.h" #include "core/ceres_wrapper/create_numeric_diff_cost_function.h" @@ -84,6 +84,8 @@ class CeresManager : public SolverManager void check(); + const Eigen::SparseMatrixd computeHessian() const; + protected: std::string solveImpl(const ReportVerbosity report_level) override; diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h index 5295bc0ac17f941503a8f28e462962290a9ee82b..80f6516415e28ddce47ab272415604014ffb0a66 100644 --- a/include/core/common/params_base.h +++ b/include/core/common/params_base.h @@ -1,7 +1,7 @@ #ifndef PARAMS_BASE_H_ #define PARAMS_BASE_H_ -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" namespace wolf { struct ParamsBase diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h index eb55faae757f7fe1b02d2fe29c75a049e0b04299..0743548b7b4aad0517ecdd2590955c4cdf7c023c 100644 --- a/include/core/factor/factor_block_absolute.h +++ b/include/core/factor/factor_block_absolute.h @@ -41,7 +41,7 @@ class FactorBlockAbsolute : public FactorAnalytic ProcessorBasePtr _processor_ptr = nullptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : - FactorAnalytic("BLOCK ABS", + FactorAnalytic("FactorBlockAbsolute", nullptr, nullptr, nullptr, diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h index 8f62b63838d79c6a654c80120bff4e4a144f4098..d3558ef21aa6e6577fc54d26015db3f9e3193312 100644 --- a/include/core/factor/factor_block_difference.h +++ b/include/core/factor/factor_block_difference.h @@ -37,8 +37,12 @@ class FactorBlockDifference : public FactorAnalytic * */ FactorBlockDifference( - StateBlockPtr _sb1_ptr, - StateBlockPtr _sb2_ptr, + const StateBlockPtr& _sb1_ptr, + const StateBlockPtr& _sb2_ptr, + const FrameBasePtr& _frame_other = nullptr, + const CaptureBasePtr& _cap_other = nullptr, + const FeatureBasePtr& _feat_other = nullptr, + const LandmarkBasePtr& _lmk_other = nullptr, unsigned int _start_idx1 = 0, int _size1 = -1, unsigned int _start_idx2 = 0, @@ -46,11 +50,11 @@ class FactorBlockDifference : public FactorAnalytic ProcessorBasePtr _processor_ptr = nullptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : - FactorAnalytic("BLOCK ABS", - nullptr, - nullptr, - nullptr, - nullptr, + FactorAnalytic("FactorBlockDifference", + _frame_other, + _cap_other, + _feat_other, + _lmk_other, _processor_ptr, _apply_loss_function, _status, @@ -146,10 +150,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size"); assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match"); assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match"); - - // normalized jacobian - _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; - _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; + assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size"); + assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size"); + + // normalized jacobian, computed according to the _compute_jacobian flag + if (_compute_jacobian[0]){ + _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; + } + if (_compute_jacobian[1]){ + _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; + } } inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector, @@ -163,10 +173,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size"); assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match"); assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match"); - - // normalized jacobian - _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; - _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; + assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size"); + assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size"); + + // normalized jacobian, computed according to the _compute_jacobian flag + if (_compute_jacobian[0]){ + _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; + } + if (_compute_jacobian[1]){ + _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; + } } inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 63674826d08a026767aeb19a62803b46625b0d2b..f2b19ef05c22f7a4c6777f889031dbda45d80a5b 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -18,7 +18,7 @@ struct ProcessorParamsBase; #include "core/common/wolf.h" #include "core/frame/frame_base.h" #include "core/state_block/state_block.h" -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" #include "core/sensor/sensor_factory.h" #include "core/processor/processor_factory.h" #include "core/processor/is_motion.h" @@ -46,8 +46,7 @@ class Problem : public std::enable_shared_from_this<Problem> HardwareBasePtr hardware_ptr_; TrajectoryBasePtr trajectory_ptr_; MapBasePtr map_ptr_; - IsMotionPtr processor_motion_ptr_; -// IsMotionPtr is_motion_ptr_; + std::list<IsMotionPtr> processor_is_motion_list_; std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_; SizeEigen state_size_, state_cov_size_, dim_; std::map<FactorBasePtr, Notification> factor_notification_map_; @@ -147,15 +146,15 @@ class Problem : public std::enable_shared_from_this<Problem> protected: /** \brief Set the processor motion * - * Set the processor motion. + * Add a new processor of type is motion to the processor is motion list. */ - void setProcessorMotion(IsMotionPtr _processor_motion_ptr); - IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name); - void clearProcessorMotion(); + void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr); + void clearProcessorIsMotion(IsMotionPtr proc); public: - IsMotionPtr& getProcessorMotion(); - + IsMotionPtr getProcessorIsMotion(); + std::list<IsMotionPtr> getProcessorIsMotionList(); + // Trajectory branch ---------------------------------- TrajectoryBasePtr getTrajectory() const; virtual FrameBasePtr setPrior(const Eigen::VectorXd& _prior_state, // @@ -271,10 +270,10 @@ class Problem : public std::enable_shared_from_this<Problem> // Zero state provider Eigen::VectorXd zeroState ( ) const; bool priorIsSet() const; + void setPriorIsSet(bool _prior_is_set); // Perturb state void perturb(double amplitude = 0.01); - // Map branch ----------------------------------------- MapBasePtr getMap() const; void loadMap(const std::string& _filename_dot_yaml); @@ -350,7 +349,7 @@ class Problem : public std::enable_shared_from_this<Problem> bool constr_by = false, // bool metric = true, // bool state_blocks = false) const; - bool check(int verbose_level) const; + bool check(int verbose_level = 0) const; bool check(bool verbose, std::ostream& stream) const; }; @@ -367,9 +366,22 @@ inline bool Problem::priorIsSet() const return prior_is_set_; } -inline IsMotionPtr& Problem::getProcessorMotion() +inline void Problem::setPriorIsSet(bool _prior_is_set) +{ + prior_is_set_ = _prior_is_set; +} + +inline IsMotionPtr Problem::getProcessorIsMotion() +{ + if (!processor_is_motion_list_.empty()) + return processor_is_motion_list_.front(); + return nullptr; +} + + +inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList() { - return processor_motion_ptr_; + return processor_is_motion_list_; } inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap() diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h index f3cf0d781f39c9fda6ffb9e64817ceed0acfa162..8c99fe72e37698a2210e35a3af0c14bf9168f65e 100644 --- a/include/core/processor/is_motion.h +++ b/include/core/processor/is_motion.h @@ -43,6 +43,12 @@ class IsMotion TimeStamp getCurrentTimeStamp() const; Eigen::VectorXd getState(const TimeStamp& _ts) const; + std::string getStateStructure(){return state_structure_;}; + void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;}; + + protected: + std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) + }; } diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index f3feeb56a3bcc6787d8efeb1355175b989dd3f3e..dc62abcac8375fc238f7c93617a33ccb8662d246 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -117,13 +117,18 @@ public: * * elements are ordered from most recent to oldest */ - std::map<TimeStamp,T> getContainer(); + const std::map<TimeStamp,T>& getContainer(); /**\brief Remove all packs in the buffer with a time stamp older than the specified * */ void removeUpTo(const TimeStamp& _time_stamp); + /**\brief Remove all packs in the buffer with a time stamp older than the specified + * + */ + void removeUpToLower(const TimeStamp& _time_stamp); + /**\brief Clear the buffer * */ @@ -503,7 +508,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element) } template <typename T> -std::map<TimeStamp,T> Buffer<T>::getContainer() +const std::map<TimeStamp,T>& Buffer<T>::getContainer() { return container_; } @@ -533,6 +538,13 @@ inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp) container_.erase(container_.begin(), post); // erasing by range } +template <typename T> +inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp) +{ + Buffer::Iterator post = container_.lower_bound(_time_stamp); + container_.erase(container_.begin(), post); // erasing by range +} + template <typename T> inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2) diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 2ac6c8d5949ad07c1529fd06739c0ce80211f243..81a9184ad4798ccbaac1ebd61a194f6e287331b4 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -13,7 +13,7 @@ #include "core/processor/processor_base.h" #include "core/processor/is_motion.h" #include "core/common/time_stamp.h" -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" // std #include <iomanip> @@ -148,6 +148,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion // This is the main public interface public: ProcessorMotion(const std::string& _type, + std::string _state_structure, int _dim, SizeEigen _state_size, SizeEigen _delta_size, @@ -505,11 +506,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!"); // ensure proper size of the provided reference - _x.resize( getProblem()->getFrameStructureSize() ); + Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_); + _x.resize( curr_x.size() ); // do get timestamp and state corrected by possible self-calibrations double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp(); - statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x); + statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x); } inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h index 6d46ab61a32c1b9573f386c97f1ae671086478c6..b03416837ec1b0a6a86a67ff0cf40fdf820b1370 100644 --- a/include/core/processor/processor_odom_2d.h +++ b/include/core/processor/processor_odom_2d.h @@ -12,7 +12,7 @@ #include "core/capture/capture_odom_2d.h" #include "core/factor/factor_odom_2d.h" #include "core/math/rotations.h" -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" namespace wolf { diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/sensor_factory.h index 3930176443a22cebb8c9591253af7f58b0a40a2c..daf01499ac759d799517cdd73ab830fb6ccaf4d5 100644 --- a/include/core/sensor/sensor_factory.h +++ b/include/core/sensor/sensor_factory.h @@ -16,7 +16,7 @@ struct ParamsSensorBase; // wolf #include "core/common/factory.h" -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" namespace wolf { diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h index d6a431d3102b0a82a218ee71247552066c1ef8eb..45499f5c56fb342ed8a82b8521e6c2e156d06031 100644 --- a/include/core/sensor/sensor_odom_2d.h +++ b/include/core/sensor/sensor_odom_2d.h @@ -3,7 +3,7 @@ //wolf includes #include "core/sensor/sensor_base.h" -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" namespace wolf { diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h index 67efe1e9af27f8456401c400766dcbf32f44057b..3d08375b4fc30e43f07f201a5653d5e83a258381 100644 --- a/include/core/sensor/sensor_odom_3d.h +++ b/include/core/sensor/sensor_odom_3d.h @@ -10,7 +10,7 @@ //wolf includes #include "core/sensor/sensor_base.h" -#include "core/utils/params_server.hpp" +#include "core/utils/params_server.h" namespace wolf { diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index 5561e2dfdf51b13dbb6e104398478570a005f5a5..07c6d757bc09125c4a4e8a55aefd6d37b480c7d4 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -15,6 +15,10 @@ namespace wolf { +/// State of nodes containing several state blocks +typedef std::unordered_map<std::string, Eigen::VectorXd> State; + + class HasStateBlocks { public: @@ -65,11 +69,13 @@ class HasStateBlocks void removeStateBlocks(ProblemPtr _problem); // States - virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true); - Eigen::VectorXd getState() const; - void getState(Eigen::VectorXd& _state) const; - unsigned int getSize() const; - unsigned int getLocalSize() const; + inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true); + void getState(Eigen::VectorXd& _state, std::string structure="") const; + Eigen::VectorXd getState(std::string structure="") const; + unsigned int getSize(std::string _sub_structure="") const; + unsigned int getLocalSize(std::string _sub_structure="") const; + + State getStateComposite(); // Perturb state void perturb(double amplitude = 0.01); @@ -201,55 +207,86 @@ inline bool HasStateBlocks::isFixed() const return fixed; } -inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _notify) +inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify) { - int size = getSize(); - + if (_sub_structure == ""){ + _sub_structure = structure_; + } + int size = getSize(_sub_structure); assert(_state.size() == size && "In FrameBase::setState wrong state size"); unsigned int index = 0; - for (const char key : getStructure()) + for (const char key : _sub_structure) { const auto& sb = getStateBlock(key); - + if (!sb){ + WOLF_ERROR("Stateblock key ", key, " not in the structure"); + } sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver index += sb->getSize(); } } -inline void HasStateBlocks::getState(Eigen::VectorXd& _state) const +// _sub_structure can be either stateblock structure of the node or a subset of this structure +inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const { - _state.resize(getSize()); + if (_sub_structure == ""){ + _sub_structure = structure_; + } + _state.resize(getSize(_sub_structure)); unsigned int index = 0; - for (const char key : getStructure()) + for (const char key : _sub_structure) { const auto& sb = getStateBlock(key); - + if (!sb){ + WOLF_ERROR("Stateblock key ", key, " not in the structure"); + } _state.segment(index,sb->getSize()) = sb->getState(); index += sb->getSize(); } } -inline Eigen::VectorXd HasStateBlocks::getState() const +inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const { Eigen::VectorXd state; - getState(state); + getState(state, _sub_structure); return state; } -inline unsigned int HasStateBlocks::getSize() const +inline State HasStateBlocks::getStateComposite() { + State state; + for (auto& pair_key_kf : state_block_map_) + { + state.emplace(pair_key_kf.first, pair_key_kf.second->getState()); + } + return state; +} + +inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const +{ + if (_sub_structure == ""){ + _sub_structure = structure_; + } unsigned int size = 0; - for (const auto& pair_key_sb : getStateBlockMap()) - size += pair_key_sb.second->getSize(); + for (const char key : _sub_structure) + { + const auto& sb = getStateBlock(key); + if (!sb){ + WOLF_ERROR("Stateblock key ", key, " not in the structure"); + } + size += sb->getSize(); + } + return size; } +//<<<<<<< HEAD inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const { const auto& it = std::find_if(state_block_map_.begin(), @@ -288,13 +325,27 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& } } -inline unsigned int HasStateBlocks::getLocalSize() const +//inline unsigned int HasStateBlocks::getLocalSize() const +//======= +inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const +//>>>>>>> devel { + if (_sub_structure == ""){ + _sub_structure = structure_; + } unsigned int size = 0; - for (const auto& pair_key_sb : getStateBlockMap()) - size += pair_key_sb.second->getLocalSize(); + for (const char key : _sub_structure) + { + const auto& sb = getStateBlock(key); + if (!sb){ + WOLF_ERROR("Stateblock key ", key, " not in the structure"); + } + size += sb->getLocalSize(); + } + return size; } } // namespace wolf + #endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */ diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h index 33aa6fcb4b221ea1dc23f24b21f4a0fb12cbfc0d..ab3bb7147f3c81f57a3436216412274394acc04a 100644 --- a/include/core/state_block/state_block.h +++ b/include/core/state_block/state_block.h @@ -74,6 +74,10 @@ public: **/ Eigen::VectorXd getState() const; + /** \brief Returns the state vector data array + **/ + double* getStateData(); + /** \brief Sets the state vector **/ void setState(const Eigen::VectorXd& _state, const bool _notify = true); @@ -283,6 +287,11 @@ inline void StateBlock::resetLocalParamUpdated() local_param_updated_.store(false); } +inline double* StateBlock::getStateData() +{ + return state_.data(); +} + }// namespace wolf #endif diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h new file mode 100644 index 0000000000000000000000000000000000000000..6f1a4cc68c2d0dcb20a6ef18f53ec259507fc176 --- /dev/null +++ b/include/core/utils/loader.h @@ -0,0 +1,32 @@ +#ifndef LOADER_H +#define LOADER_H + +#include <string> + +class Loader{ +protected: + std::string path_; +public: + Loader(std::string _file); + virtual void load() = 0; + virtual void close() = 0; +}; + +class LoaderRaw: public Loader{ + void* resource_; +public: + LoaderRaw(std::string _file); + ~LoaderRaw(); + void load(); + void close(); +}; +// class LoaderPlugin: public Loader{ +// ClassLoader* resource_; +// void load(){ +// this->resource_ = new ClassLoader(this->path_); +// } +// void close(){ +// delete this->resource_; +// } +// }; +#endif diff --git a/include/core/utils/loader.hpp b/include/core/utils/loader.hpp deleted file mode 100644 index 3d867d799b36bfd53fb42e93577554caf1b050c1..0000000000000000000000000000000000000000 --- a/include/core/utils/loader.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef LOADER_HPP -#define LOADER_HPP -#include <dlfcn.h> -#include <string> -#include <stdexcept> -class Loader{ -protected: - std::string path_; -public: - Loader(std::string _file){ - this->path_ = _file; - } - virtual void load() = 0; - virtual void close() = 0; -}; -class LoaderRaw: public Loader{ - void* resource_; -public: - LoaderRaw(std::string _file): - Loader(_file) - { - // - } - ~LoaderRaw(){ - this->close(); - } - void load(){ - this->resource_ = dlopen(this->path_.c_str(), RTLD_LAZY); - if(not this->resource_) - throw std::runtime_error("Couldn't load resource with path " + this->path_ + "\n" + "Error info: " + dlerror()); - } - void close(){ - if(this->resource_) dlclose(this->resource_); - } -}; -// class LoaderPlugin: public Loader{ -// ClassLoader* resource_; -// void load(){ -// this->resource_ = new ClassLoader(this->path_); -// } -// void close(){ -// delete this->resource_; -// } -// }; -#endif \ No newline at end of file diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h new file mode 100644 index 0000000000000000000000000000000000000000..e8b473ca26703f17ea932cca4d1e913717108026 --- /dev/null +++ b/include/core/utils/params_server.h @@ -0,0 +1,55 @@ +#ifndef PARAMS_SERVER_H +#define PARAMS_SERVER_H + +#include "core/utils/converter.h" + +#include <map> +#include <exception> + +namespace wolf{ + +class MissingValueException : public std::runtime_error +{ +public: + MissingValueException(std::string _msg) : std::runtime_error(_msg) {} +}; + +class ParamsServer{ + std::map<std::string, std::string> params_; +public: + ParamsServer(); + ParamsServer(std::map<std::string, std::string> _params); + ~ParamsServer(){ + // + } + + void print(); + + + void addParam(std::string _key, std::string _value); + + void addParams(std::map<std::string, std::string> _params); + + // template<typename T> + // T getParam(std::string key, std::string def_value) const { + // if(params_.find(key) != params_.end()){ + // return converter<T>::convert(params_.find(key)->second); + // }else{ + // return converter<T>::convert(def_value); + // } + // } + + template<typename T> + T getParam(std::string _key) const { + if(params_.find(_key) != params_.end()){ + return converter<T>::convert(params_.find(_key)->second); + }else{ + throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server."); + } + } + +}; + +} + +#endif diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp deleted file mode 100644 index a750eab79f4ee90b39e4e21d1926129897c29d92..0000000000000000000000000000000000000000 --- a/include/core/utils/params_server.hpp +++ /dev/null @@ -1,70 +0,0 @@ -#ifndef PARAMS_SERVER_HPP -#define PARAMS_SERVER_HPP - -#include "core/utils/converter.h" -//#include "core/yaml/parser_yaml.hpp" - -#include <vector> -#include <regex> -#include <map> -#include <exception> - -namespace wolf{ - -class MissingValueException : public std::runtime_error -{ -public: - MissingValueException(std::string msg) : std::runtime_error(msg) {} -}; - -class ParamsServer{ - std::map<std::string, std::string> _params; -public: - ParamsServer(){ - _params = std::map<std::string, std::string>(); - } - ParamsServer(std::map<std::string, std::string> params){ - _params = params; - } - ~ParamsServer(){ - // - } - - void print(){ - for(auto it : _params) - std::cout << it.first << "~~" << it.second << std::endl; - } - - - void addParam(std::string key, std::string value){ - _params.insert(std::pair<std::string, std::string>(key, value)); - } - - void addParams(std::map<std::string, std::string> params) - { - _params.insert(params.begin(), params.end()); - } - - // template<typename T> - // T getParam(std::string key, std::string def_value) const { - // if(_params.find(key) != _params.end()){ - // return converter<T>::convert(_params.find(key)->second); - // }else{ - // return converter<T>::convert(def_value); - // } - // } - - template<typename T> - T getParam(std::string key) const { - if(_params.find(key) != _params.end()){ - return converter<T>::convert(_params.find(key)->second); - }else{ - throw MissingValueException("The following key: '" + key + "' has not been found in the parameters server."); - } - } - -}; - -} - -#endif diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h new file mode 100644 index 0000000000000000000000000000000000000000..1b6f659ebd609dffef4f3c472800cf32714f22e1 --- /dev/null +++ b/include/core/yaml/parser_yaml.h @@ -0,0 +1,80 @@ +#ifndef PARSER_YAML_H +#define PARSER_YAML_H + +#include "core/utils/converter.h" +#include "core/common/wolf.h" + +#include "yaml-cpp/yaml.h" + +namespace wolf +{ +class ParserYAML { + struct ParamsInitSensor{ + std::string type_; + std::string name_; + std::string plugin_; + YAML::Node n_; + }; + struct ParamsInitProcessor{ + std::string type_; + std::string name_; + std::string name_assoc_sensor_; + std::string plugin_; + YAML::Node n_; + }; + struct SubscriberManager{ + std::string package_; + std::string subscriber_; + std::string topic_; + std::string sensor_name_; + YAML::Node n_; + }; + struct PublisherManager{ + std::string subscriber_; + std::string topic_; + std::string period_; + YAML::Node n_; + }; + std::map<std::string, std::string> params_; + std::string active_name_; + std::vector<ParamsInitSensor> paramsSens_; + std::vector<ParamsInitProcessor> paramsProc_; + std::stack<std::string> parsing_file_; + std::string file_; + std::string path_root_; + std::vector<SubscriberManager> subscriber_managers_; + std::vector<PublisherManager> publisher_managers_; + YAML::Node problem; + std::string generatePath(std::string); + YAML::Node loadYAML(std::string); + void insert_register(std::string, std::string); +public: + ParserYAML(std::string _file, std::string _path_root = "", + bool _freely_parse = false); + ~ParserYAML() + { + // + } + void parse_freely(); + std::map<std::string, std::string> getParams(); + + private: + void walkTree(std::string _file); + void walkTree(std::string _file, std::vector<std::string>& _tags); + void walkTree(std::string _file, std::vector<std::string>& _tags, std::string _hdr); +/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file + * @param YAML node to be parsed + * @param tags represents the path from the root of the YAML tree to the current node + * @param hdr is the name of the current YAML node + */ + void walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string _hdr); + void updateActiveName(std::string _tag); +/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements + are defined at the top level of the YAML file. + * @param file is the path to the YAML file */ + void parseFirstLevel(std::string _file); + std::string tagsToString(std::vector<std::string>& _tags); + void parse(); +}; +} +#endif diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp deleted file mode 100644 index 22f584d76bab2aa2c3a6bd6ca30edbeb4873a4ea..0000000000000000000000000000000000000000 --- a/include/core/yaml/parser_yaml.hpp +++ /dev/null @@ -1,532 +0,0 @@ -#ifndef PARSER_YAML_HPP -#define PARSER_YAML_HPP - -#include "core/utils/converter.h" -#include "core/common/wolf.h" - -#include "yaml-cpp/yaml.h" - -#include <string> -#include <vector> -#include <list> -#include <stack> -#include <regex> -#include <map> -#include <iostream> -#include <algorithm> -#include <numeric> - -namespace { - //====== START OF FORWARD DECLARATION ======== - std::string parseAtomicNode(YAML::Node); - std::string fetchMapEntry(YAML::Node); - std::string mapToString(std::map<std::string,std::string>); - //====== END OF FORWARD DECLARATION ======== - -/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences. - * @param n the node representing a map - * @return std::map<std::string, std::string> populated with the key,value pairs in n - */ -std::map<std::string, std::string> fetchAsMap(YAML::Node n){ - assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node"); - auto m = std::map<std::string, std::string>(); - for(const auto& kv : n){ - std::string key = kv.first.as<std::string>(); - switch (kv.second.Type()) { - case YAML::NodeType::Scalar : { - std::string value = kv.second.Scalar(); - m.insert(std::pair<std::string,std::string>(key, value)); - break; - } - case YAML::NodeType::Sequence : { - std::string aux = parseAtomicNode(kv.second); - m.insert(std::pair<std::string,std::string>(key, aux)); - break; - } - case YAML::NodeType::Map : { - std::string value = fetchMapEntry(kv.second); - std::regex r("^\\$.*"); - if (std::regex_match(key, r)) key = key.substr(1,key.size()-1); - m.insert(std::pair<std::string,std::string>(key, value)); - break; - } - default: - assert(1 == 0 && "Unsupported node Type at fetchAsMap"); - break; - } - } - return m; -} - std::string fetchMapEntry(YAML::Node n){ - switch (n.Type()) { - case YAML::NodeType::Scalar: { - return n.Scalar(); - break; - } - case YAML::NodeType::Sequence: { - return parseAtomicNode(n); - break; - } - case YAML::NodeType::Map: { - return mapToString(fetchAsMap(n)); - break; - } - default: { - assert(1 == 0 && "Unsupported node Type at fetchMapEntry"); - return ""; - break; - } - } - } - /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...] - * @param _map just a std::map<std::string,std::string> - * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...] - */ - std::string mapToString(std::map<std::string,std::string> _map){ - std::string result = ""; - auto v = std::vector<std::string>(); - std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";}); - auto concat = [](std::string ac, std::string str)-> std::string { - return ac + str + ","; - }; - std::string aux = ""; - std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat); - if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1); - else accumulated = ""; - return "[" + accumulated + "]"; - } - /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars. - * @param n a vector of YAML::Node that represents a YAML::Sequence - * @return <b>{std::string}</b> representing the YAML sequence - */ - std::string parseAtomicNode(YAML::Node n){ - std::string aux = ""; - bool first = true; - std::string separator = ""; - switch(n.Type()){ - case YAML::NodeType::Scalar: - return n.Scalar(); - break; - case YAML::NodeType::Sequence: - for(auto it : n){ - aux += separator + parseAtomicNode(it); - if(first){ - separator = ","; - first = false; - } - } - return "[" + aux + "]"; - break; - case YAML::NodeType::Map: - return mapToString(fetchAsMap(n)); - break; - default: - return ""; - break; - } - } - - /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type - * Scalar, Sequence or Map. - * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map - * @param n node to be test for atomicity - */ - bool isAtomic(std::string key, YAML::Node n){ - assert(n.Type() != YAML::NodeType::Undefined && n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node"); - std::regex r("^\\$.*"); - bool is_atomic = true; - switch(n.Type()){ - case YAML::NodeType::Scalar: - return true; - break; - case YAML::NodeType::Sequence: - for(auto it : n) { - switch(it.Type()){ - case YAML::NodeType::Map: - for(const auto& kv : it){ - is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it); - } - break; - default: - is_atomic = is_atomic and isAtomic("", it); - break; - } - } - return is_atomic; - break; - case YAML::NodeType::Map: - is_atomic = std::regex_match(key, r); - return is_atomic; - break; - default: - throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(n.Type())); - return false; - break; - } - return false; - } -} -class ParserYAML { - struct ParamsInitSensor{ - std::string _type; - std::string _name; - std::string _plugin; - YAML::Node n; - }; - struct ParamsInitProcessor{ - std::string _type; - std::string _name; - std::string _name_assoc_sensor; - std::string _plugin; - YAML::Node n; - }; - struct SubscriberManager{ - std::string _package; - std::string _subscriber; - std::string _topic; - std::string _sensor_name; - YAML::Node n; - }; - struct PublisherManager{ - std::string _subscriber; - std::string _topic; - std::string _period; - YAML::Node n; - }; - std::map<std::string, std::string> _params; - std::string _active_name; - std::vector<ParamsInitSensor> _paramsSens; - std::vector<ParamsInitProcessor> _paramsProc; - std::stack<std::string> _parsing_file; - std::string _file; - std::string _path_root; - std::vector<SubscriberManager> _subscriber_managers; - std::vector<PublisherManager> _publisher_managers; - YAML::Node problem; - std::string generatePath(std::string); - YAML::Node loadYAML(std::string); - void insert_register(std::string, std::string); -public: - ParserYAML(std::string file, std::string path_root = "", - bool freely_parse = false) { - _params = std::map<std::string, std::string>(); - _active_name = ""; - _paramsSens = std::vector<ParamsInitSensor>(); - _paramsProc = std::vector<ParamsInitProcessor>(); - _subscriber_managers = std::vector<SubscriberManager>(); - _publisher_managers = std::vector<PublisherManager>(); - _parsing_file = std::stack<std::string>(); - _file = file; - if (path_root != "") { - std::regex r(".*/ *$"); - if (not std::regex_match(path_root, r)) - _path_root = path_root + "/"; - else - _path_root = path_root; - } - if(not freely_parse) this->parse(); - else this->parse_freely(); - } - ~ParserYAML() - { - // - } - void parse_freely(); - std::map<std::string, std::string> getParams(); - - private: - void walkTree(std::string file); - void walkTree(std::string file, std::vector<std::string>& tags); - void walkTree(std::string file, std::vector<std::string>& tags, std::string hdr); - void walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr); - void updateActiveName(std::string tag); - void parseFirstLevel(std::string file); - std::string tagsToString(std::vector<std::string>& tags); - void parse(); -}; -std::string ParserYAML::generatePath(std::string file){ - std::regex r("^/.*"); - if(std::regex_match(file, r)){ - return file; - }else{ - return _path_root + file; - } -} -YAML::Node ParserYAML::loadYAML(std::string file){ - try{ - // std::cout << "Parsing " << generatePath(file) << std::endl; - WOLF_INFO("Parsing file: ", generatePath(file)); - return YAML::LoadFile(generatePath(file)); - }catch (YAML::BadFile& e){ - throw std::runtime_error("Couldn't load file " + generatePath(file) + ". Tried to open it from " + this->_parsing_file.top()); - } -} -std::string ParserYAML::tagsToString(std::vector<std::string> &tags){ - std::string hdr = ""; - for(auto it : tags){ - hdr = hdr + "/" + it; - } - return hdr; -} -void ParserYAML::walkTree(std::string file){ - YAML::Node n; - n = loadYAML(generatePath(file)); - this->_parsing_file.push(generatePath(file)); - std::vector<std::string> hdrs = std::vector<std::string>(); - walkTreeR(n, hdrs, ""); - this->_parsing_file.pop(); -} -void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags){ - YAML::Node n; - n = loadYAML(generatePath(file)); - this->_parsing_file.push(generatePath(file)); - walkTreeR(n, tags, ""); - this->_parsing_file.pop(); -} -void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){ - YAML::Node n; - n = loadYAML(generatePath(file)); - this->_parsing_file.push(generatePath(file)); - walkTreeR(n, tags, hdr); - this->_parsing_file.pop(); -} -/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file -* @param YAML node to be parsed -* @param tags represents the path from the root of the YAML tree to the current node -* @param hdr is the name of the current YAML node -*/ -void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr){ - switch (n.Type()) { - case YAML::NodeType::Scalar : { - std::regex r("^@.*"); - if(std::regex_match(n.Scalar(), r)){ - std::string str = n.Scalar(); - walkTree(str.substr(1,str.size() - 1), tags, hdr); - }else{ - insert_register(hdr, n.Scalar()); - } - break; - } - case YAML::NodeType::Sequence : { - if(isAtomic("", n)){ - insert_register(hdr, parseAtomicNode(n)); - }else{ - for(const auto& kv : n){ - walkTreeR(kv, tags, hdr); - } - } - break; - } - case YAML::NodeType::Map : { - for(const auto& kv : n){ - if(isAtomic(kv.first.as<std::string>(), n)){ - std::string key = kv.first.as<std::string>(); - //WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key); - key = key.substr(1,key.size() - 1); - insert_register(hdr + "/" + key, parseAtomicNode(kv.second)); - }else{ - /* - If key=="follow" then the parser will assume that the value is a path and will parse - the (expected) yaml file at the specified path. Note that this does not increase the header depth. - The following example shows how the header remains unafected: - @my_main_config | @some_path - - cov_det: 1 | - my_value : 23 - - follow: "@some_path" | - - var: 1.2 | - Resulting map: - cov_det -> 1 - my_value-> 23 - var: 1.2 - Instead of: - cov_det -> 1 - follow/my_value-> 23 - var: 1.2 - Which would result from the following yaml files - @my_main_config | @some_path - - cov_det: 1 | - my_value : 23 - - $follow: "@some_path" | - - var: 1.2 | - */ - std::string key = kv.first.as<std::string>(); - //WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key); - std::regex rr("follow"); - if(not std::regex_match(kv.first.as<std::string>(), rr)) { - tags.push_back(kv.first.as<std::string>()); - if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>()); - walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>()); - tags.pop_back(); - if(tags.size() == 1) this->updateActiveName(""); - }else{ - walkTree(kv.second.as<std::string>(), tags, hdr); - } - } - } - break; - } - case YAML::NodeType::Null : - break; - default: - assert(1 == 0 && "Unsupported node Type at walkTreeR."); - break; - } -} -void ParserYAML::updateActiveName(std::string tag){ - this->_active_name = tag; -} -/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements - are defined at the top level of the YAML file. - * @param file is the path to the YAML file */ -void ParserYAML::parseFirstLevel(std::string file){ - YAML::Node n; - n = loadYAML(generatePath(file)); - - YAML::Node n_config = n["config"]; - // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node"); - if(n_config.Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find config node. Please make sure that your YAML file " + generatePath(file) + " starts with 'config:'"); - if(n_config["problem"].Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " + generatePath(file) + " has a 'problem' entry"); - this->problem = n_config["problem"]; - std::vector<std::map<std::string, std::string>> map_container; - try{ - for(const auto& kv : n_config["sensors"]){ - ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv}; - _paramsSens.push_back(pSensor); - map_container.push_back(std::map<std::string, std::string>({ - {"type", kv["type"].Scalar()}, - {"name", kv["name"].Scalar()}, - {"plugin", kv["plugin"].Scalar()} - })); - } - insert_register("sensors", wolf::converter<std::string>::convert(map_container)); - map_container.clear(); - } catch(YAML::InvalidNode& e){ - throw std::runtime_error("Error parsing sensors @" + generatePath(file) + ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries."); - } - - try{ - for(const auto& kv : n_config["processors"]){ - ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv}; - _paramsProc.push_back(pProc); - map_container.push_back(std::map<std::string, std::string>({ - {"type", kv["type"].Scalar()}, - {"name", kv["name"].Scalar()}, - {"plugin", kv["plugin"].Scalar()}, - {"sensor_name", kv["sensor_name"].Scalar()}})); - } - insert_register("processors", - wolf::converter<std::string>::convert(map_container)); - map_container.clear(); - } catch(YAML::InvalidNode& e){ - throw std::runtime_error("Error parsing processors @" + generatePath(file) + ". Please make sure that each processor has 'type', 'name', 'plugin' and 'sensor_name' entries."); - } - try { - for (const auto &kv : n_config["ROS subscriber"]) { - SubscriberManager pSubscriberManager = {kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv}; - _subscriber_managers.push_back(pSubscriberManager); - map_container.push_back(std::map<std::string, std::string>({ - {"package", kv["package"].Scalar()}, - {"type", kv["type"].Scalar()}, - {"topic", kv["topic"].Scalar()}, - {"sensor_name", kv["sensor_name"].Scalar()}})); - } - insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container)); - map_container.clear(); - } catch (YAML::InvalidNode &e) { - throw std::runtime_error("Error parsing subscriber @" + generatePath(file) + ". Please make sure that each manager has 'package', 'type', 'topic' and 'sensor_name' entries."); - } - - try { - for (const auto &kv : n_config["ROS publisher"]) { - WOLF_DEBUG("WHAT"); - PublisherManager pPublisherManager = {kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv}; - _publisher_managers.push_back(pPublisherManager); - map_container.push_back(std::map<std::string, std::string>({ - {"type", kv["type"].Scalar()}, - {"topic", kv["topic"].Scalar()}, - {"period", kv["period"].Scalar()}})); - } - insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container)); - map_container.clear(); - } catch (YAML::InvalidNode &e) { - throw std::runtime_error("Error parsing publisher @" + generatePath(file) + ". Please make sure that each manager has 'type', 'topic' and 'period' entries."); - } -} -std::map<std::string,std::string> ParserYAML::getParams(){ - std::map<std::string,std::string> rtn = _params; - return rtn; -} -void ParserYAML::parse(){ - this->_parsing_file.push(generatePath(this->_file)); - this->parseFirstLevel(this->_file); - - if(this->problem.Type() != YAML::NodeType::Undefined){ - std::vector<std::string> tags = std::vector<std::string>(); - this->walkTreeR(this->problem, tags , "problem"); - } - for(auto it : _paramsSens){ - std::vector<std::string> tags = std::vector<std::string>(); - tags.push_back("sensor"); - this->walkTreeR(it.n , tags , "sensor/" + it._name); - } - for(auto it : _paramsProc){ - std::vector<std::string> tags = std::vector<std::string>(); - tags.push_back("processor"); - this->walkTreeR(it.n , tags , "processor/" + it._name); - } - std::list<std::string> plugins, packages; - for (const auto& it : this->_paramsSens) - plugins.push_back(it._plugin); - for (const auto& it : this->_paramsProc) - plugins.push_back(it._plugin); - for (const auto& it : this->_subscriber_managers) - packages.push_back(it._package); - plugins.sort(); - plugins.unique(); - packages.sort(); - packages.unique(); - insert_register("plugins", wolf::converter<std::string>::convert(plugins)); - insert_register("packages", wolf::converter<std::string>::convert(packages)); - - YAML::Node n; - n = loadYAML(generatePath(this->_file)); - - if (n.Type() == YAML::NodeType::Map) - { - for (auto it : n) - { - auto node = it.second; - // WOLF_INFO("WUT ", it.first); - std::vector<std::string> tags = std::vector<std::string>(); - if(it.first.as<std::string>() != "config") - this->walkTreeR(node, tags, it.first.as<std::string>()); - else - { - for (auto itt : node) - { - std::string node_key = itt.first.as<std::string>(); - if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and - node_key != "ROS subscriber" and node_key != "ROS publisher") - { - this->walkTreeR(itt.second, tags, node_key); - } - } - } - } - }else - { - std::vector<std::string> tags = std::vector<std::string>(); - this->walkTreeR(n, tags, ""); - } - this->_parsing_file.pop(); - } -void ParserYAML::parse_freely(){ - this->_parsing_file.push(generatePath(this->_file)); - std::vector<std::string> tags = std::vector<std::string>(); - this->walkTreeR(loadYAML(this->_file), tags, ""); - this->_parsing_file.pop(); -} -void ParserYAML::insert_register(std::string key, std::string value){ - if(key.substr(0,1) == "/") key = key.substr(1,key.size()-1); - auto inserted_it = _params.insert(std::pair<std::string, std::string>(key, value)); - if(not inserted_it.second) WOLF_WARN("Skipping key '", key, "' with value '", value, "'. There already exists the register: (", inserted_it.first->first, ",", inserted_it.first->second, ")"); -} -#endif diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index ecf57e7d7e29b4b8ccfecf09f8c39d8eacdafdbb..0d8cba6f97cb7e2c2cdf067e7ebcb65d8ac79c10 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -443,6 +443,77 @@ void CeresManager::check() } } +void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col) +{ + for (auto ins_row = 0; ins_row < ins.rows(); ins_row++) + for (auto ins_col = 0; ins_col < ins.cols(); ins_col++) + original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col); + + original.makeCompressed(); +} + +const Eigen::SparseMatrixd CeresManager::computeHessian() const +{ + Eigen::SparseMatrixd H; + Eigen::SparseMatrixd A; + // fac_2_residual_idx_ + // fac_2_costfunction_ + // state_blocks_local_param_ + int ix_row = 0; // index of the factor/measurement in the + for (auto fac_res_pair: fac_2_residual_idx_){ + FactorBasePtr fac_ptr = fac_res_pair.first; + ix_row += fac_ptr->getSize(); + + // retrieve all stateblocks data related to this factor + std::vector<const double*> fac_states_ptr; + for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){ + fac_states_ptr.push_back(sb->getStateData()); + } + + // retrieve residual value, not used afterwards in this case since we just care about jacobians + Eigen::VectorXd residual(fac_ptr->getSize()); + // retrieve jacobian in group size, not local size + std::vector<Eigen::MatrixXd> jacobians; + fac_ptr->evaluate(fac_states_ptr, residual, jacobians); + + // Retrieve the block row sparse matrix of this factor + Eigen::SparseMatrixd A_block_row(fac_ptr->getSize(), A.cols()); + for (auto i = 0; i < jacobians.size(); i++) + { + StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i]; + if (!sb->isFixed()) + { + assert((state_blocks_local_param_.find(sb) != state_blocks_local_param_.end()) && "factor involving a state block not added"); + assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols()) - 1 && "bad A number of cols"); + + // insert since A_block_row has just been created so it's empty for sure + if (sb->hasLocalParametrization()){ + // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian + Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize()); + Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize()); + Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize()); + + sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map); + insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col) + } + else { + insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize()); + } + } + } + + // fill the weighted jacobian matrix block row + A.block(ix_row, 0, fac_ptr->getSize(), A.cols()); + + } + + // compute the hessian matrix from the weighted jacobian + H = A.transpose() * A; + + return H; +} + + } // namespace wolf #include "core/solver/solver_factory.h" namespace wolf { diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index e7fabaefc24245604cad750d19461d124a231432..c0f4d310e1062c9a68ea94a0ee0b70f912cd9d15 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -64,14 +64,14 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c time_stamp_(_ts) { if(_frame_structure.compare("PO") == 0 and _dim == 2){ - assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2d!"); + assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for PO 2D!"); StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) ); StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((double) _x(2) ) ); addStateBlock("P", p_ptr); addStateBlock("O", o_ptr); this->setType("PO 2d"); }else if(_frame_structure.compare("PO") == 0 and _dim == 3){ - assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3d!"); + assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for PO 3D!"); StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) ); StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) ); addStateBlock("P", p_ptr); @@ -79,7 +79,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c this->setType("PO 3d"); }else if(_frame_structure.compare("POV") == 0 and _dim == 3){ // auto _x = Eigen::VectorXd::Zero(10); - assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3d!"); + assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 3D!"); StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) ); StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) ); StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) ); @@ -87,6 +87,22 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c addStateBlock("O", o_ptr); addStateBlock("V", v_ptr); this->setType("POV 3d"); + }else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){ + // auto _x = Eigen::VectorXd::Zero(10); + assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!"); + StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) ); + StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3 ) ) ); + StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.segment <3> (7 ) ) ); + StateBlockPtr c_ptr ( std::make_shared<StateBlock> (_x.segment <3> (10) ) ); + StateBlockPtr cd_ptr ( std::make_shared<StateBlock> (_x.segment <3> (13) ) ); + StateBlockPtr Lc_ptr ( std::make_shared<StateBlock> (_x.segment <3> (16) ) ); + addStateBlock("P", p_ptr); + addStateBlock("O", o_ptr); + addStateBlock("V", v_ptr); + addStateBlock("C", c_ptr); + addStateBlock("D", cd_ptr); + addStateBlock("L", Lc_ptr); + this->setType("POVCDL 3d"); }else{ std::cout << _frame_structure << " ^^ " << _dim << std::endl; throw std::runtime_error("Unknown frame structure"); diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index ccee9e6bf80f67e897db1ec84225457b96a9eff2..1d72f5b8fff0b187d192e575eb140a2e442f101e 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -12,8 +12,8 @@ #include "core/processor/processor_factory.h" #include "core/state_block/state_block.h" #include "core/utils/logging.h" -#include "core/utils/params_server.hpp" -#include "core/utils/loader.hpp" +#include "core/utils/params_server.h" +#include "core/utils/loader.h" #include "core/utils/check_log.hpp" @@ -35,25 +35,27 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) : hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)), map_ptr_(std::make_shared<MapBase>()), - processor_motion_ptr_(), + processor_is_motion_list_(std::list<IsMotionPtr>()), prior_is_set_(false), frame_structure_(_frame_structure) { + dim_ = _dim; if (_frame_structure == "PO" and _dim == 2) { state_size_ = 3; state_cov_size_ = 3; - dim_ = 2; }else if (_frame_structure == "PO" and _dim == 3) { state_size_ = 7; state_cov_size_ = 6; - dim_ = 3; - } else if (_frame_structure == "POV" and _dim == 3) + }else if (_frame_structure == "POV" and _dim == 3) { state_size_ = 10; state_cov_size_ = 9; - dim_ = 3; + }else if (_frame_structure == "POVCDL" and _dim == 3) + { + state_size_ = 19; + state_cov_size_ = 18; } else std::runtime_error( "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement."); @@ -235,10 +237,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // if (prc_ptr->isMotion() && prior_is_set_) (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); - // setting the main processor motion - if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr) - processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr); - return prc_ptr; } @@ -284,10 +282,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // if (prc_ptr->isMotion() && prior_is_set_) (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); - // setting the main processor motion - if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr) - processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr); - return prc_ptr; } @@ -306,47 +300,6 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const return (*sen_it); } -IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name) -{ - for (auto sen : getHardware()->getSensorList()) // loop all sensors - { - auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name - sen->getProcessorList().end(), - [&](ProcessorBasePtr prc) - { - return prc->getName() == _processor_name; - }); // lambda function for the find_if - - if (prc_it != sen->getProcessorList().end()) // found something! - { - if ((*prc_it)->isMotion()) // found, and it's motion! - { - std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl; - processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it); - return processor_motion_ptr_; - } - else // found, but it's not motion! - { - std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl; - return nullptr; - } - } - } - // nothing found! - std::cout << "Processor '" << _processor_name << "' not found!" << std::endl; - return nullptr; -} - -void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr) -{ - processor_motion_ptr_ = _processor_motion_ptr; -} - -void Problem::clearProcessorMotion() -{ - processor_motion_ptr_.reset(); -} - FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, // const SizeEigen _dim, // FrameType _frame_key_type, // @@ -387,41 +340,108 @@ Eigen::VectorXd Problem::getCurrentState() const void Problem::getCurrentState(Eigen::VectorXd& _state) const { - if (processor_motion_ptr_ != nullptr) - processor_motion_ptr_->getCurrentState(_state); - else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr) - trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state); - else - _state = zeroState(); + TimeStamp ts; // throwaway timestamp + getCurrentStateAndStamp(_state, ts); } -void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) const -{ - if (processor_motion_ptr_ != nullptr) +void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const +{ + if (!processor_is_motion_list_.empty()) { - processor_motion_ptr_->getCurrentState(_state); - processor_motion_ptr_->getCurrentTimeStamp(ts); + // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state) + std::list<TimeStamp> proc_is_motion_current_ts; + for (auto proc: processor_is_motion_list_){ + proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp()); + } + auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end()); + getState(*min_it, _state); + _ts = *min_it; } else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr) { - trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts); + // kind of redundant with getState(_ts, _state) + trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts); trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state); } else _state = zeroState(); } + +// Problem of this implmentation: if more state void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const { - // try to get the state from processor_motion if any, otherwise... - if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state)) - { - FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); + // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp + FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); + if (processor_is_motion_list_.empty()){ if (closest_frame != nullptr) - closest_frame->getState(_state); + _state = closest_frame->getState(); else _state = zeroState(); } + + // RETRIEVE FROM PROCESSOR MOTION + // TODO: current implementation messy, would be much easier with a state being an std::unordered_map + else { + // Iterate over the problem state structure and get the corresponding state + // in the first processor is motion that provides it + // finally check if the state to concatenate list has the same total size as the problem state_size + + // an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors + std::unordered_map<char, Eigen::VectorXd> states_to_concat_map; // not necessary to be ordered + for (auto proc: processor_is_motion_list_){ + Eigen::VectorXd proc_state = proc->getState(_ts); + + int idx = 0; + for (char sb_name: proc->getStateStructure()){ + // not already there + if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){ + if (sb_name == 'O'){ + int size_sb = dim_ == 3 ? 4 : 1; // really bad: should be more transparent + states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb); + idx += size_sb; + } + else{ + int size_sb = dim_ == 3 ? 3 : 2; + states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb); + idx += size_sb; + } + } + } + } + + int concat_size = 0; + for (auto state_map_it: states_to_concat_map){ + concat_size += state_map_it.second.size(); + } + // assert(concat_size == state_size_ && "Problem with the concatenated size: " ); + + // fill the state value from the state concatenation in the order dictated by frame_structure_ + int idx = 0; + _state.resize(state_size_); + for (char sb_name: frame_structure_){ + Eigen::VectorXd sb_state; + int size_sb; // really bad... + if (sb_name == 'O'){ + size_sb = dim_ == 3 ? 4 : 1; + } + else { + size_sb = dim_ == 3 ? 3 : 2; + } + if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){ + sb_state = states_to_concat_map[sb_name]; + } + else { + // Should be taken from the last state but too messy already + sb_state.resize(size_sb); + sb_state.setZero(); + } + + _state.segment(idx, size_sb) = sb_state; + idx += size_sb; + + } + } } Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const @@ -451,6 +471,18 @@ std::string Problem::getFrameStructure() const return frame_structure_; } +void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr) +{ + processor_is_motion_list_.push_back(_processor_motion_ptr); +} + +void Problem::clearProcessorIsMotion(IsMotionPtr proc){ + auto it = std::find(processor_is_motion_list_.begin(), processor_is_motion_list_.end(), proc); + if (it != processor_is_motion_list_.end()){ + processor_is_motion_list_.erase(it); + } +} + Eigen::VectorXd Problem::zeroState() const { Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize()); @@ -846,10 +878,16 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen: // create origin capture with the given state as data // Capture fix only takes 3d position and Quaternion orientation CapturePosePtr init_capture; - if (this->getFrameStructure() == "POV" and this->getDim() == 3) + // if (this->getFrameStructure() == "POV" and this->getDim() == 3) + // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); + // else + // init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov); + + if (this->getDim() == 3) init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); else - init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov); + init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(3), _prior_cov.topLeftCorner(3,3)); + // emplace feature and factor init_capture->emplaceFeatureAndFactor(); @@ -1827,9 +1865,12 @@ void Problem::perturb(double amplitude) // Frames and Captures for (auto& F : getTrajectory()->getFrameList()) { - F->perturb(amplitude); - for (auto& C : F->getCaptureList()) - C->perturb(amplitude); + if (F->isKeyOrAux()) + { + F->perturb(amplitude); + for (auto& C : F->getCaptureList()) + C->perturb(amplitude); + } } // Landmarks diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index a35097f2c70d3b047117ae6d0c9bf81a5fe2f5da..f61f861b021e51a08dd92db301aa1bd8de159272 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -53,12 +53,13 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture"); WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp()); - // if trigger, process directly without buffering - if (triggerInCapture(_capture_ptr)) - processCapture(_capture_ptr); // asking if capture should be stored if (storeCapture(_capture_ptr)) buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr); + + // if trigger, process directly without buffering + if (triggerInCapture(_capture_ptr)) + processCapture(_capture_ptr); } void ProcessorBase::remove() @@ -68,12 +69,12 @@ void ProcessorBase::remove() is_removing_ = true; ProcessorBasePtr this_p = shared_from_this(); - // clear Problem::processor_motion_ptr_ if (isMotion()) { ProblemPtr P = getProblem(); - if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) ) - P->clearProcessorMotion(); + auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() ); + if(P && this_proc_cast_attempt ) + P->clearProcessorIsMotion(this_proc_cast_attempt); } // remove from upstream diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 634d1f6d1284997573ef763bd329dcdf37cc94d3..aded1532cc17fc6a0f2e0d48edbbca857a1ab28b 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -7,6 +7,7 @@ namespace wolf { ProcessorMotion::ProcessorMotion(const std::string& _type, + std::string _state_structure, int _dim, SizeEigen _state_size, SizeEigen _delta_size, @@ -25,7 +26,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, origin_ptr_(), last_ptr_(), incoming_ptr_(), - dt_(0.0), x_(_state_size), + dt_(0.0), + x_(_state_size), delta_(_delta_size), delta_cov_(_delta_cov_size, _delta_cov_size), delta_integrated_(_delta_size), @@ -34,7 +36,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, jacobian_delta_preint_(delta_cov_size_, delta_cov_size_), jacobian_delta_(delta_cov_size_, delta_cov_size_), jacobian_calib_(delta_cov_size_, calib_size_) -{ +{ + setStateStructure(_state_structure); // } @@ -267,7 +270,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // Update state and time stamps last_ptr_->setTimeStamp(getCurrentTimeStamp()); last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp()); - last_ptr_->getFrame()->setState(getCurrentState()); + last_ptr_->getFrame()->setState(getProblem()->getState(getCurrentTimeStamp())); if (permittedKeyFrame() && voteForKeyFrame()) { @@ -315,7 +318,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // create a new frame auto frame_new = getProblem()->emplaceFrame(NON_ESTIMATED, - getCurrentState(), + getProblem()->getCurrentState(), getCurrentTimeStamp()); // create a new capture auto capture_new = emplaceCapture(frame_new, @@ -346,6 +349,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) postProcess(); } +// _x needs to have the size of the processor state bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const { CaptureMotionPtr capture_motion; @@ -360,7 +364,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const { // Get origin state and calibration CaptureBasePtr cap_orig = capture_motion->getOriginCapture(); - VectorXd state_0 = cap_orig->getFrame()->getState(); + VectorXd state_0 = cap_orig->getFrame()->getState(state_structure_); VectorXd calib = cap_orig->getCalibration(); // Get delta and correct it with new calibration params @@ -371,7 +375,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step); // ensure proper size of the provided reference - _x.resize( getProblem()->getFrameStructureSize() ); + _x.resize( state_0.size() ); // Compose on top of origin state using the buffered time stamp, not the query time stamp double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; @@ -380,7 +384,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const else { // We could not find any CaptureMotion for the time stamp requested - WOLF_ERROR("Could not find any Capture for the time stamp requested. "); + WOLF_ERROR("Could not find any Capture for the time stamp requested. ", _ts); WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?") return false; } @@ -415,9 +419,11 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame) // ---------- LAST ---------- // Make non-key-frame for last Capture + TimeStamp origin_ts = _origin_frame->getTimeStamp(); auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED, - _origin_frame->getState(), - _origin_frame->getTimeStamp()); + _origin_frame->getState(), + origin_ts); + // emplace (emtpy) last Capture last_ptr_ = emplaceCapture(new_frame_ptr, getSensor(), @@ -562,7 +568,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp ++frame_rev_iter) { frame = *frame_rev_iter; - capture = frame->getCaptureOf(getSensor()); + auto sensor = getSensor(); + capture = frame->getCaptureOf(sensor); if (capture != nullptr) { // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion @@ -621,6 +628,9 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() void ProcessorMotion::setProblem(ProblemPtr _problem) { + std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D"; + assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str()); + if (_problem == nullptr or _problem == this->getProblem()) return; @@ -629,11 +639,10 @@ void ProcessorMotion::setProblem(ProblemPtr _problem) // set the origin if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr) this->setOrigin(this->getProblem()->getLastKeyFrame()); - - // set the main processor motion - if (this->getProblem()->getProcessorMotion() == nullptr) - this->getProblem()->setProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this())); -}; + + // adding processor is motion to the processor is motion vector + getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this())); +} bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr) { diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index c986ebf7cf9cd7fb83df1417ea19c42beed66c74..929c1ed7ceb825fdaa265a1af592fa8125832789 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -6,7 +6,7 @@ namespace wolf { ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) : - ProcessorMotion("ProcessorOdom2d", 2, 3, 3, 3, 2, 0, _params), + ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params), params_odom_2d_(_params) { unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity(); diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp index 967031e9ccd1a68fcee41567be0f7dbf7523ef2c..3e1268eb1d705b3a335d9c8d9389e0c9449ebba1 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/src/processor/processor_odom_3d.cpp @@ -3,7 +3,7 @@ namespace wolf { ProcessorOdom3d::ProcessorOdom3d(ProcessorParamsOdom3dPtr _params) : - ProcessorMotion("ProcessorOdom3d", 3, 7, 7, 6, 6, 0, _params), + ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params), params_odom_3d_ (_params), k_disp_to_disp_ (0), k_disp_to_rot_ (0), diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9fdf6984b6a3de57fa8a8ae062232a685897209f --- /dev/null +++ b/src/utils/loader.cpp @@ -0,0 +1,37 @@ +#include "core/utils/loader.h" + +#include <dlfcn.h> +#include <stdexcept> + +Loader::Loader(std::string _file) +{ + path_ = _file; +} +LoaderRaw::LoaderRaw(std::string _file) : Loader(_file) +{ + // +} +LoaderRaw::~LoaderRaw() +{ + close(); +} +void LoaderRaw::load() +{ + resource_ = dlopen(path_.c_str(), RTLD_LAZY); + if (not resource_) + throw std::runtime_error("Couldn't load resource with path " + path_ + "\n" + "Error info: " + dlerror()); +} +void LoaderRaw::close() +{ + if (resource_) + dlclose(resource_); +} +// class LoaderPlugin: public Loader{ +// ClassLoader* resource_; +// void load(){ +// resource_ = new ClassLoader(path_); +// } +// void close(){ +// delete resource_; +// } +// }; diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6344466792706c9e499695721f95e781089ec07e --- /dev/null +++ b/src/utils/params_server.cpp @@ -0,0 +1,28 @@ +#include "core/utils/params_server.h" + +using namespace wolf; + +ParamsServer::ParamsServer() +{ + params_ = std::map<std::string, std::string>(); +} +ParamsServer::ParamsServer(std::map<std::string, std::string> _params) +{ + params_ = _params; +} + +void ParamsServer::print() +{ + for (auto it : params_) + std::cout << it.first << "~~" << it.second << std::endl; +} + +void ParamsServer::addParam(std::string _key, std::string _value) +{ + params_.insert(std::pair<std::string, std::string>(_key, _value)); +} + +void ParamsServer::addParams(std::map<std::string, std::string> _params) +{ + params_.insert(_params.begin(), _params.end()); +} diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..267d0c9da2024abd59ed5b529d13de7680d14472 --- /dev/null +++ b/src/yaml/parser_yaml.cpp @@ -0,0 +1,547 @@ +#include "core/yaml/parser_yaml.h" + +#include <string> +#include <vector> +#include <list> +#include <stack> +#include <regex> +#include <map> +#include <iostream> +#include <algorithm> +#include <numeric> + +using namespace wolf; +namespace { + //====== START OF FORWARD DECLARATION ======== + std::string parseAtomicNode(YAML::Node); + std::string fetchMapEntry(YAML::Node); + std::string mapToString(std::map<std::string,std::string>); + //====== END OF FORWARD DECLARATION ======== + +/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences. + * @param n the node representing a map + * @return std::map<std::string, std::string> populated with the key,value pairs in n + */ +std::map<std::string, std::string> fetchAsMap(YAML::Node _n){ + assert(_n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node"); + auto m = std::map<std::string, std::string>(); + for(const auto& kv : _n){ + std::string key = kv.first.as<std::string>(); + switch (kv.second.Type()) { + case YAML::NodeType::Scalar : { + std::string value = kv.second.Scalar(); + m.insert(std::pair<std::string,std::string>(key, value)); + break; + } + case YAML::NodeType::Sequence : { + std::string aux = parseAtomicNode(kv.second); + m.insert(std::pair<std::string,std::string>(key, aux)); + break; + } + case YAML::NodeType::Map : { + std::string value = fetchMapEntry(kv.second); + std::regex r("^\\$.*"); + if (std::regex_match(key, r)) key = key.substr(1,key.size()-1); + m.insert(std::pair<std::string,std::string>(key, value)); + break; + } + default: + assert(1 == 0 && "Unsupported node Type at fetchAsMap"); + break; + } + } + return m; +} + std::string fetchMapEntry(YAML::Node _n){ + switch (_n.Type()) { + case YAML::NodeType::Scalar: { + return _n.Scalar(); + break; + } + case YAML::NodeType::Sequence: { + return parseAtomicNode(_n); + break; + } + case YAML::NodeType::Map: { + return mapToString(fetchAsMap(_n)); + break; + } + default: { + assert(1 == 0 && "Unsupported node Type at fetchMapEntry"); + return ""; + break; + } + } + } + /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...] + * @param map_ just a std::map<std::string,std::string> + * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...] + */ + std::string mapToString(std::map<std::string,std::string> _map){ + std::string result = ""; + auto v = std::vector<std::string>(); + std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";}); + auto concat = [](std::string ac, std::string str)-> std::string { + return ac + str + ","; + }; + std::string aux = ""; + std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat); + if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1); + else accumulated = ""; + return "[" + accumulated + "]"; + } + /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars. + * @param n a vector of YAML::Node that represents a YAML::Sequence + * @return <b>{std::string}</b> representing the YAML sequence + */ + std::string parseAtomicNode(YAML::Node _n){ + std::string aux = ""; + bool first = true; + std::string separator = ""; + switch(_n.Type()){ + case YAML::NodeType::Scalar: + return _n.Scalar(); + break; + case YAML::NodeType::Sequence: + for(auto it : _n){ + aux += separator + parseAtomicNode(it); + if(first){ + separator = ","; + first = false; + } + } + return "[" + aux + "]"; + break; + case YAML::NodeType::Map: + return mapToString(fetchAsMap(_n)); + break; + default: + return ""; + break; + } + } + + /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type + * Scalar, Sequence or Map. + * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map + * @param n node to be test for atomicity + */ + bool isAtomic(std::string _key, YAML::Node _n){ + assert(_n.Type() != YAML::NodeType::Undefined && _n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node"); + std::regex r("^\\$.*"); + bool is_atomic = true; + switch(_n.Type()){ + case YAML::NodeType::Scalar: + return true; + break; + case YAML::NodeType::Sequence: + for(auto it : _n) { + switch(it.Type()){ + case YAML::NodeType::Map: + for(const auto& kv : it){ + is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it); + } + break; + default: + is_atomic = is_atomic and isAtomic("", it); + break; + } + } + return is_atomic; + break; + case YAML::NodeType::Map: + is_atomic = std::regex_match(_key, r); + return is_atomic; + break; + default: + throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(_n.Type())); + return false; + break; + } + return false; + } +} +ParserYAML::ParserYAML(std::string _file, std::string path_root, bool freely_parse) +{ + params_ = std::map<std::string, std::string>(); + active_name_ = ""; + paramsSens_ = std::vector<ParamsInitSensor>(); + paramsProc_ = std::vector<ParamsInitProcessor>(); + subscriber_managers_ = std::vector<SubscriberManager>(); + publisher_managers_ = std::vector<PublisherManager>(); + parsing_file_ = std::stack<std::string>(); + file_ = _file; + if (path_root != "") + { + std::regex r(".*/ *$"); + if (not std::regex_match(path_root, r)) + path_root_ = path_root + "/"; + else + path_root_ = path_root; + } + if (not freely_parse) + parse(); + else + parse_freely(); +} + +std::string ParserYAML::generatePath(std::string _file) +{ + std::regex r("^/.*"); + if (std::regex_match(_file, r)) + { + return _file; + } + else + { + return path_root_ + _file; + } +} +YAML::Node ParserYAML::loadYAML(std::string _file) +{ + try + { + // std::cout << "Parsing " << generatePath(_file) << std::endl; + WOLF_INFO("Parsing file: ", generatePath(_file)); + return YAML::LoadFile(generatePath(_file)); + } + catch (YAML::BadFile& e) + { + throw std::runtime_error("Couldn't load file " + generatePath(_file) + ". Tried to open it from " + + parsing_file_.top()); + } +} +std::string ParserYAML::tagsToString(std::vector<std::string>& _tags) +{ + std::string hdr = ""; + for (auto it : _tags) + { + hdr = hdr + "/" + it; + } + return hdr; +} +void ParserYAML::walkTree(std::string _file) +{ + YAML::Node n; + n = loadYAML(generatePath(_file)); + parsing_file_.push(generatePath(_file)); + std::vector<std::string> hdrs = std::vector<std::string>(); + walkTreeR(n, hdrs, ""); + parsing_file_.pop(); +} +void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags) +{ + YAML::Node n; + n = loadYAML(generatePath(_file)); + parsing_file_.push(generatePath(_file)); + walkTreeR(n, _tags, ""); + parsing_file_.pop(); +} +void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags, std::string hdr) +{ + YAML::Node n; + n = loadYAML(generatePath(_file)); + parsing_file_.push(generatePath(_file)); + walkTreeR(n, _tags, hdr); + parsing_file_.pop(); +} +void ParserYAML::walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string hdr) +{ + switch (_n.Type()) + { + case YAML::NodeType::Scalar: + { + std::regex r("^@.*"); + if (std::regex_match(_n.Scalar(), r)) + { + std::string str = _n.Scalar(); + walkTree(str.substr(1, str.size() - 1), _tags, hdr); + } + else + { + insert_register(hdr, _n.Scalar()); + } + break; + } + case YAML::NodeType::Sequence: + { + if (isAtomic("", _n)) + { + insert_register(hdr, parseAtomicNode(_n)); + } + else + { + for (const auto& kv : _n) + { + walkTreeR(kv, _tags, hdr); + } + } + break; + } + case YAML::NodeType::Map: + { + for (const auto& kv : _n) + { + if (isAtomic(kv.first.as<std::string>(), _n)) + { + std::string key = kv.first.as<std::string>(); + // WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key); + key = key.substr(1, key.size() - 1); + insert_register(hdr + "/" + key, parseAtomicNode(kv.second)); + } + else + { + /* + If key=="follow" then the parser will assume that the value is a path and will parse + the (expected) yaml file at the specified path. Note that this does not increase the header depth. + The following example shows how the header remains unafected: + @my_main_config | @some_path + - cov_det: 1 | - my_value : 23 + - follow: "@some_path" | + - var: 1.2 | + Resulting map: + cov_det -> 1 + my_value-> 23 + var: 1.2 + Instead of: + cov_det -> 1 + follow/my_value-> 23 + var: 1.2 + Which would result from the following yaml files + @my_main_config | @some_path + - cov_det: 1 | - my_value : 23 + - $follow: "@some_path" | + - var: 1.2 | + */ + std::string key = kv.first.as<std::string>(); + // WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key); + std::regex rr("follow"); + if (not std::regex_match(kv.first.as<std::string>(), rr)) + { + _tags.push_back(kv.first.as<std::string>()); + if (_tags.size() == 2) + updateActiveName(kv.first.as<std::string>()); + walkTreeR(kv.second, _tags, hdr + "/" + kv.first.as<std::string>()); + _tags.pop_back(); + if (_tags.size() == 1) + updateActiveName(""); + } + else + { + walkTree(kv.second.as<std::string>(), _tags, hdr); + } + } + } + break; + } + case YAML::NodeType::Null: + break; + default: + assert(1 == 0 && "Unsupported node Type at walkTreeR."); + break; + } +} +void ParserYAML::updateActiveName(std::string _tag) +{ + active_name_ = _tag; +} +void ParserYAML::parseFirstLevel(std::string _file) +{ + YAML::Node n; + n = loadYAML(generatePath(_file)); + + YAML::Node n_config = n["config"]; + // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node"); + if (n_config.Type() != YAML::NodeType::Map) + throw std::runtime_error("Could not find config node. Please make sure that your YAML file " + + generatePath(_file) + " starts with 'config:'"); + if (n_config["problem"].Type() != YAML::NodeType::Map) + throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " + + generatePath(_file) + " has a 'problem' entry"); + problem = n_config["problem"]; + std::vector<std::map<std::string, std::string>> map_container; + try + { + for (const auto& kv : n_config["sensors"]) + { + ParamsInitSensor pSensor = { kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv }; + paramsSens_.push_back(pSensor); + map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() }, + { "name", kv["name"].Scalar() }, + { "plugin", kv["plugin"].Scalar() } })); + } + insert_register("sensors", wolf::converter<std::string>::convert(map_container)); + map_container.clear(); + } + catch (YAML::InvalidNode& e) + { + throw std::runtime_error("Error parsing sensors @" + generatePath(_file) + + ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries."); + } + + try + { + for (const auto& kv : n_config["processors"]) + { + ParamsInitProcessor pProc = { + kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv + }; + paramsProc_.push_back(pProc); + map_container.push_back( + std::map<std::string, std::string>({ { "type", kv["type"].Scalar() }, + { "name", kv["name"].Scalar() }, + { "plugin", kv["plugin"].Scalar() }, + { "sensor_name", kv["sensor_name"].Scalar() } })); + } + insert_register("processors", wolf::converter<std::string>::convert(map_container)); + map_container.clear(); + } + catch (YAML::InvalidNode& e) + { + throw std::runtime_error("Error parsing processors @" + generatePath(_file) + + ". Please make sure that each processor has 'type', 'name', 'plugin' and " + "'sensor_name' entries."); + } + try + { + for (const auto& kv : n_config["ROS subscriber"]) + { + SubscriberManager pSubscriberManager = { + kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv + }; + subscriber_managers_.push_back(pSubscriberManager); + map_container.push_back( + std::map<std::string, std::string>({ { "package", kv["package"].Scalar() }, + { "type", kv["type"].Scalar() }, + { "topic", kv["topic"].Scalar() }, + { "sensor_name", kv["sensor_name"].Scalar() } })); + } + insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container)); + map_container.clear(); + } + catch (YAML::InvalidNode& e) + { + throw std::runtime_error("Error parsing subscriber @" + generatePath(_file) + + ". Please make sure that each manager has 'package', 'type', 'topic' and " + "'sensor_name' entries."); + } + + try + { + for (const auto& kv : n_config["ROS publisher"]) + { + WOLF_DEBUG("WHAT"); + PublisherManager pPublisherManager = { + kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv + }; + publisher_managers_.push_back(pPublisherManager); + map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() }, + { "topic", kv["topic"].Scalar() }, + { "period", kv["period"].Scalar() } })); + } + insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container)); + map_container.clear(); + } + catch (YAML::InvalidNode& e) + { + throw std::runtime_error("Error parsing publisher @" + generatePath(_file) + + ". Please make sure that each manager has 'type', 'topic' and 'period' entries."); + } +} +std::map<std::string, std::string> ParserYAML::getParams() +{ + std::map<std::string, std::string> rtn = params_; + return rtn; +} +void ParserYAML::parse() +{ + parsing_file_.push(generatePath(file_)); + parseFirstLevel(file_); + + if (problem.Type() != YAML::NodeType::Undefined) + { + std::vector<std::string> tags = std::vector<std::string>(); + walkTreeR(problem, tags, "problem"); + } + for (auto it : paramsSens_) + { + std::vector<std::string> tags = std::vector<std::string>(); + tags.push_back("sensor"); + walkTreeR(it.n_, tags, "sensor/" + it.name_); + } + for (auto it : paramsProc_) + { + std::vector<std::string> tags = std::vector<std::string>(); + tags.push_back("processor"); + walkTreeR(it.n_, tags, "processor/" + it.name_); + } + std::list<std::string> plugins, packages; + for (const auto& it : paramsSens_) + plugins.push_back(it.plugin_); + for (const auto& it : paramsProc_) + plugins.push_back(it.plugin_); + for (const auto& it : subscriber_managers_) + packages.push_back(it.package_); + plugins.sort(); + plugins.unique(); + packages.sort(); + packages.unique(); + insert_register("plugins", wolf::converter<std::string>::convert(plugins)); + insert_register("packages", wolf::converter<std::string>::convert(packages)); + + YAML::Node n; + n = loadYAML(generatePath(file_)); + + if (n.Type() == YAML::NodeType::Map) + { + for (auto it : n) + { + auto node = it.second; + // WOLF_INFO("WUT ", it.first); + std::vector<std::string> tags = std::vector<std::string>(); + if (it.first.as<std::string>() != "config") + walkTreeR(node, tags, it.first.as<std::string>()); + else + { + for (auto itt : node) + { + std::string node_key = itt.first.as<std::string>(); + if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and + node_key != "ROS subscriber" and node_key != "ROS publisher") + { + walkTreeR(itt.second, tags, node_key); + } + } + } + } + } + else + { + std::vector<std::string> tags = std::vector<std::string>(); + walkTreeR(n, tags, ""); + } + parsing_file_.pop(); +} +void ParserYAML::parse_freely() +{ + parsing_file_.push(generatePath(file_)); + std::vector<std::string> tags = std::vector<std::string>(); + walkTreeR(loadYAML(file_), tags, ""); + parsing_file_.pop(); +} +void ParserYAML::insert_register(std::string _key, std::string _value) +{ + if (_key.substr(0, 1) == "/") + _key = _key.substr(1, _key.size() - 1); + auto inserted_it = params_.insert(std::pair<std::string, std::string>(_key, _value)); + if (not inserted_it.second) + WOLF_WARN("Skipping key '", + _key, + "' with value '", + _value, + "'. There already exists the register: (", + inserted_it.first->first, + ",", + inserted_it.first->second, + ")"); +} diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp index 20bbe2c42ed27688820569a3da345596da3903f1..1e06c7a80377b99197a721e85473319f9bf34862 100644 --- a/test/gtest_factor_block_difference.cpp +++ b/test/gtest_factor_block_difference.cpp @@ -50,18 +50,17 @@ class FixtureFactorBlockDifference : public testing::Test Vector10d x_origin = problem_->zeroState(); Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity(); - KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1); + KF0_ = problem_->setPrior(x_origin, cov_prior, t0, 0.1); CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0); FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>()); FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV()); - // KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0); KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1); - Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "DIFF", t1); + Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1); Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity(); - Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov); + Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov); } virtual void TearDown() override {} @@ -150,6 +149,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx) Feat_->setMeasurementCovariance(cov_diff); FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( Feat_, KF0_->getP(), KF1_->getP(), + nullptr, nullptr, nullptr, nullptr, 0, 1, 0, 1 ); @@ -174,6 +174,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy) Feat_->setMeasurementCovariance(cov_diff); FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( Feat_, KF0_->getP(), KF1_->getP(), + nullptr, nullptr, nullptr, nullptr, 0, 2, 0, 2 ); @@ -195,6 +196,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz) Feat_->setMeasurementCovariance(cov_diff); FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>( Feat_, KF0_->getP(), KF1_->getP(), + nullptr, nullptr, nullptr, nullptr, 1, 2, 1, 2 ); diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp index 18f6b1eded0955faf7c6b823e25bbde62894203d..1f1f468360e1aa5073606f8ea32e7d82cafbc438 100644 --- a/test/gtest_param_server.cpp +++ b/test/gtest_param_server.cpp @@ -1,8 +1,8 @@ #include "core/utils/utils_gtest.h" #include "core/utils/converter.h" #include "core/common/wolf.h" -#include "core/yaml/parser_yaml.hpp" -#include "core/utils/params_server.hpp" +#include "core/yaml/parser_yaml.h" +#include "core/utils/params_server.h" using namespace std; using namespace wolf; diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp index c8702619bc5150eb688bcb7dccfa11f71ab909e5..de5164ec6a9db61ad8a0344e8e6ee6d6e52dec16 100644 --- a/test/gtest_parser_yaml.cpp +++ b/test/gtest_parser_yaml.cpp @@ -1,7 +1,7 @@ #include "core/utils/utils_gtest.h" #include "core/utils/converter.h" #include "core/common/wolf.h" -#include "core/yaml/parser_yaml.hpp" +#include "core/yaml/parser_yaml.h" using namespace std; using namespace wolf; diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index d25f1451492f90d90613b5a08882ee02ae9c3f4d..6d262ba06b81438499b29ab3d45fd4bb101c89db 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -98,7 +98,7 @@ TEST(Problem, Processor) ProblemPtr P = Problem::create("PO", 3); // check motion processor is null - ASSERT_FALSE(P->getProcessorMotion()); + ASSERT_FALSE(P->getProcessorIsMotion()); // add a motion sensor and processor auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d()); @@ -107,7 +107,7 @@ TEST(Problem, Processor) auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ProcessorParamsOdom3d>()); // check motion processor IS NOT by emplace - ASSERT_EQ(P->getProcessorMotion(), Pm); + ASSERT_EQ(P->getProcessorIsMotion(), Pm); } TEST(Problem, Installers) @@ -122,16 +122,16 @@ TEST(Problem, Installers) auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer"); // check motion processor IS NOT set - ASSERT_FALSE(P->getProcessorMotion()); + ASSERT_FALSE(P->getProcessorIsMotion()); // install processor motion ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml"); // check motion processor is set - ASSERT_TRUE(P->getProcessorMotion()); + ASSERT_TRUE(P->getProcessorIsMotion() != nullptr); // check motion processor is correct - ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorMotion()), pm); + ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorIsMotion()), pm); } TEST(Problem, SetOrigin_PO_2d) diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 1933da8599234e61c534e9f05008710ac4396661..c4a1c4b0470c92bf09df1a950a1c19e57d35642d 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -125,7 +125,7 @@ TEST(ProcessorBase, KeyFrameCallback) capt_trk = make_shared<CaptureVoid>(t, sens_trk); proc_trk->captureCallback(capt_trk); -// problem->print(4,1,1,0); + problem->print(4,1,1,0); // Only odom creating KFs ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2d")==0 );