diff --git a/CMakeLists.txt b/CMakeLists.txt index 32122e793bc0f69a9c41d941173862f4fe332468..1a4e4718320ecb5597f8c90b5ebd8efddaf364e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -197,8 +197,8 @@ SET(HDRS_PROBLEM include/core/problem/problem.h ) SET(HDRS_PROCESSOR - include/core/processor/is_motion.h include/core/processor/motion_buffer.h + include/core/processor/motion_provider.h include/core/processor/processor_base.h include/core/processor/processor_diff_drive.h include/core/processor/processor_fix_wing_model.h @@ -308,8 +308,8 @@ SET(SRCS_PROBLEM src/problem/problem.cpp ) SET(SRCS_PROCESSOR - src/processor/is_motion.cpp src/processor/motion_buffer.cpp + src/processor/motion_provider.cpp src/processor/processor_base.cpp src/processor/processor_diff_drive.cpp src/processor/processor_fix_wing_model.cpp diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index 705ebba5b41ee5b7cba77b2b20cbe2b0abcd7289..de2bf5f14e06cac0338a36e76e130fc074748437 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -116,7 +116,7 @@ int main() using namespace wolf; - WOLF_TRACE("======== CONFIGURE PROBLEM ======="); + WOLF_INFO("======== CONFIGURE PROBLEM ======="); // Config file to parse. Here is where all the problem is defined: std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml"; @@ -144,7 +144,7 @@ int main() // APPLY PRIOR and SET PROCESSOR ODOM ORIGIN =================================================== TimeStamp t(0.0); FrameBasePtr KF1 = problem->applyPriorOptions(t); - std::static_pointer_cast<ProcessorMotion>(problem->getProcessorIsMotion())->setOrigin(KF1); +// std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1); // SELF CALIBRATION =================================================== // These few lines control whether we calibrate some sensor parameters or not. @@ -169,12 +169,16 @@ int main() // SET OF EVENTS: make things happen ======================================================= std::cout << std::endl; - WOLF_TRACE("======== START ROBOT MOVE AND SLAM =======") + WOLF_INFO("======== START ROBOT MOVE AND SLAM =======") // We'll do 3 steps of motion and landmark observations. // STEP 1 -------------------------------------------------------------- + // move zero motion to accept the first keyframe and initialize the processor + CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0*motion_data, 0*motion_cov); + cap_motion ->process(); // KF1 : (0,0,0) + // observe lmks ids.resize(1); ranges.resize(1); bearings.resize(1); ids << 1; // will observe Lmk 1 @@ -187,7 +191,7 @@ int main() t += 1.0; // t : 1.0 // motion - CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov); + cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov); cap_motion ->process(); // KF2 : (1,0,0) // observe lmks @@ -220,40 +224,40 @@ int main() // SOLVE ================================================================ // SOLVE with exact initial guess - WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======") + WOLF_INFO("======== SOLVE PROBLEM WITH EXACT PRIORS =======") std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); - WOLF_TRACE(report); // should show a very low iteration number (possibly 1) + WOLF_INFO(report); // should show a very low iteration number (possibly 1) problem->print(1,0,1,0); // PERTURB initial guess - WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======") + WOLF_INFO("======== PERTURB PROBLEM PRIORS =======") problem->perturb(0.5); // Perturb all state blocks that are not fixed problem->print(1,0,1,0); // SOLVE again - WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======") + WOLF_INFO("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======") report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); - WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!) + WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!) problem->print(1,0,1,0); // GET COVARIANCES of all states - WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") + WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======") ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); for (auto& kf : *problem->getTrajectory()) { Eigen::MatrixXd cov; kf->getCovariance(cov); - WOLF_TRACE("KF", kf->id(), "_cov = \n", cov); + WOLF_INFO("KF", kf->id(), "_cov = \n", cov); } for (auto& lmk : problem->getMap()->getLandmarkList()) { Eigen::MatrixXd cov; lmk->getCovariance(cov); - WOLF_TRACE("L", lmk->id(), "_cov = \n", cov); + WOLF_INFO("L", lmk->id(), "_cov = \n", cov); } std::cout << std::endl; - WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======") + WOLF_INFO("======== FINAL PRINT FOR INTERPRETATION =======") problem->print(4,1,1,1); /* diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml index c35ecb64e9c79a5e9108284e1d4134f4cf7b37f1..c46580effcef4d12a9e4829d6a1c9cfdb8794da3 100644 --- a/demos/hello_wolf/yaml/hello_wolf_config.yaml +++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml @@ -13,7 +13,7 @@ config: $sigma: P: [0.31, 0.31] O: [0.31] - time_tolerance: 0.1 + time_tolerance: 0.5 tree_manager: type: "none" diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 8e7c83e628d9a0edc200d797ec3eeebc17286c41..5e35324c2ebc2e84838a4ba23ecf0ac208bd84a5 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -42,7 +42,7 @@ struct ParamsProcessorBase; #include "core/utils/params_server.h" #include "core/sensor/factory_sensor.h" #include "core/processor/factory_processor.h" -#include "core/processor/is_motion.h" +#include <core/processor/motion_provider.h> #include "core/state_block/state_composite.h" // std includes @@ -72,14 +72,14 @@ class Problem : public std::enable_shared_from_this<Problem> friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap()) friend ProcessorBase; friend ProcessorMotion; - friend IsMotion; + friend MotionProvider; protected: TreeManagerBasePtr tree_manager_; HardwareBasePtr hardware_ptr_; TrajectoryBasePtr trajectory_ptr_; MapBasePtr map_ptr_; - std::map<int, IsMotionPtr> processor_is_motion_map_; + std::map<int, MotionProviderPtr> motion_provider_map_; std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_; SizeEigen state_size_, state_cov_size_, dim_; std::map<FactorBasePtr, Notification> factor_notification_map_; @@ -194,13 +194,13 @@ class Problem : public std::enable_shared_from_this<Problem> * * Add a new processor of type is motion to the processor is motion list. */ - void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr); - void removeProcessorIsMotion(IsMotionPtr proc); + void addMotionProvider(MotionProviderPtr _processor_motion_ptr); + void removeMotionProvider(MotionProviderPtr proc); public: - IsMotionPtr getProcessorIsMotion(); - std::map<int,IsMotionPtr>& getProcessorIsMotionMap(); - const std::map<int,IsMotionPtr>& getProcessorIsMotionMap() const; +// MotionProviderPtr getMotionProvider(); + std::map<int,MotionProviderPtr>& getMotionProviderMap(); + const std::map<int,MotionProviderPtr>& getMotionProviderMap() const; // Trajectory branch ---------------------------------- TrajectoryBasePtr getTrajectory() const; @@ -449,21 +449,21 @@ inline bool Problem::isPriorSet() const return prior_options_ == nullptr; } -inline IsMotionPtr Problem::getProcessorIsMotion() -{ - if (not processor_is_motion_map_.empty()) - return processor_is_motion_map_.begin()->second; - return nullptr; -} +//inline MotionProviderPtr Problem::getMotionProvider() +//{ +// if (not motion_provider_map_.empty()) +// return motion_provider_map_.begin()->second; +// return nullptr; +//} -inline std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap() +inline std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap() { - return processor_is_motion_map_; + return motion_provider_map_; } -inline const std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap() const +inline const std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap() const { - return processor_is_motion_map_; + return motion_provider_map_; } inline SizeStd Problem::getStateBlockNotificationMapSize() const diff --git a/include/core/processor/is_motion.h b/include/core/processor/motion_provider.h similarity index 68% rename from include/core/processor/is_motion.h rename to include/core/processor/motion_provider.h index f0607c1f584b856fa2957c29b115c165395f41b1..da27ccb6e08fc6f22577bc5dcc6678e2b68dcfca 100644 --- a/include/core/processor/is_motion.h +++ b/include/core/processor/motion_provider.h @@ -20,14 +20,14 @@ // //--------LICENSE_END-------- /** - * \file is_motion.h + * \file motion_provider.h * * Created on: Mar 10, 2020 * \author: jsola */ -#ifndef PROCESSOR_IS_MOTION_H_ -#define PROCESSOR_IS_MOTION_H_ +#ifndef PROCESSOR_MOTION_PROVIDER_H_ +#define PROCESSOR_MOTION_PROVIDER_H_ #include "core/common/wolf.h" #include "core/state_block/state_composite.h" @@ -36,15 +36,15 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(ParamsIsMotion); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider); -struct ParamsIsMotion +struct ParamsMotionProvider { bool state_getter = true; int state_priority = 1; - ParamsIsMotion() = default; - ParamsIsMotion(std::string _unique_name, const ParamsServer& _server) + ParamsMotionProvider() = default; + ParamsMotionProvider(std::string _unique_name, const ParamsServer& _server) { state_getter = _server.getParam<bool>("processor/" + _unique_name + "/state_getter"); state_priority = _server.getParam<double>("processor/" + _unique_name + "/state_priority"); @@ -58,14 +58,14 @@ struct ParamsIsMotion }; class TimeStamp; -WOLF_PTR_TYPEDEFS(IsMotion); +WOLF_PTR_TYPEDEFS(MotionProvider); -class IsMotion +class MotionProvider { public: - IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params); - virtual ~IsMotion(); + MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params); + virtual ~MotionProvider(); // Queries to the processor: virtual TimeStamp getTimeStamp() const = 0; @@ -82,22 +82,22 @@ class IsMotion public: const StateStructure& getStateStructure ( ) { return state_structure_; }; void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; }; - void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr); + void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr); protected: StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) VectorComposite odometry_; - ParamsIsMotionPtr params_is_motion_; + ParamsMotionProviderPtr params_motion_provider_; }; -inline IsMotion::IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params) : +inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) : state_structure_(_structure), - params_is_motion_(_params) + params_motion_provider_(_params) { // } -inline wolf::VectorComposite IsMotion::getOdometry ( ) const +inline wolf::VectorComposite MotionProvider::getOdometry ( ) const { return odometry_; } @@ -107,25 +107,25 @@ inline wolf::VectorComposite IsMotion::getOdometry ( ) const ///// IMPLEMENTATION /////// namespace wolf{ -inline IsMotion::~IsMotion() +inline MotionProvider::~MotionProvider() { } -inline bool IsMotion::isStateGetter() const +inline bool MotionProvider::isStateGetter() const { - return params_is_motion_->state_getter; + return params_motion_provider_->state_getter; } -inline int IsMotion::getStatePriority() const +inline int MotionProvider::getStatePriority() const { - return params_is_motion_->state_priority; + return params_motion_provider_->state_priority; } -inline void IsMotion::setStatePriority(int _priority) +inline void MotionProvider::setStatePriority(int _priority) { - params_is_motion_->state_priority = _priority; + params_motion_provider_->state_priority = _priority; } } /* namespace wolf */ -#endif /* PROCESSOR_IS_MOTION_H_ */ +#endif /* PROCESSOR_MOTION_PROVIDER_H_ */ diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index e0c26dca49ba4e05a50303853f2426225c0cc01b..84996921ca13b9813e23724e709da3ed30afe28e 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -30,7 +30,7 @@ class SensorBase; // Wolf includes #include "core/common/wolf.h" #include "core/common/node_base.h" -#include "core/processor/is_motion.h" +#include <core/processor/motion_provider.h> #include "core/sensor/sensor_base.h" #include "core/frame/frame_base.h" #include "core/common/time_stamp.h" @@ -378,7 +378,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;} public: - bool isMotion() const; + bool isMotionProvider() const; void setTimeTolerance(double _time_tolerance); @@ -448,10 +448,10 @@ inline void ProcessorBase::setVotingActive(bool _voting_active) params_->voting_active = _voting_active; } -inline bool ProcessorBase::isMotion() const +inline bool ProcessorBase::isMotionProvider() const { - // check if this inherits from IsMotion - return (std::dynamic_pointer_cast<const IsMotion>(shared_from_this()) != nullptr); + // check if this inherits from MotionProvider + return (std::dynamic_pointer_cast<const MotionProvider>(shared_from_this()) != nullptr); } inline unsigned int ProcessorBase::id() const diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 44437086be1c96e01fb8693dd88e585cf0a5e979..7a657aaae5cb74890c2ab95cf647e689478fe0b3 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -30,9 +30,9 @@ #define PROCESSOR_MOTION_H_ // Wolf +#include <core/processor/motion_provider.h> #include "core/capture/capture_motion.h" #include "core/processor/processor_base.h" -#include "core/processor/is_motion.h" #include "core/common/time_stamp.h" #include "core/utils/params_server.h" @@ -44,7 +44,7 @@ namespace wolf WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorMotion); -struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion +struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionProvider { double max_time_span = 0.5; unsigned int max_buff_length = 10; @@ -55,7 +55,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion ParamsProcessorMotion() = default; ParamsProcessorMotion(std::string _unique_name, const ParamsServer& _server): ParamsProcessorBase(_unique_name, _server), - ParamsIsMotion(_unique_name, _server) + ParamsMotionProvider(_unique_name, _server) { max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span"); max_buff_length = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/max_buff_length"); @@ -66,7 +66,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion std::string print() const override { return ParamsProcessorBase::print() + "\n" + - ParamsIsMotion::print() + "\n" + ParamsMotionProvider::print() + "\n" + "max_time_span: " + std::to_string(max_time_span) + "\n" + "max_buff_length: " + std::to_string(max_buff_length) + "\n" + "dist_traveled: " + std::to_string(dist_traveled) + "\n" @@ -153,7 +153,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion * // TODO: JS: review instructions up to here * */ -class ProcessorMotion : public ProcessorBase, public IsMotion +class ProcessorMotion : public ProcessorBase, public MotionProvider { public: typedef enum { @@ -258,13 +258,13 @@ class ProcessorMotion : public ProcessorBase, public IsMotion * * Returns true if the key frame should be stored */ - bool storeKeyFrame(FrameBasePtr) override; + bool storeKeyFrame(FrameBasePtr) override { return true;} /** \brief store capture * * Returns true if the capture should be stored */ - bool storeCapture(CaptureBasePtr) override; + bool storeCapture(CaptureBasePtr) override { return false;} bool voteForKeyFrame() const override; diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index a3459c845a05cdb5b98d266ae7f6f3856108f41d..932b6d0ea55ff5b9a0f4df8b7457be90c8de62cc 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -159,13 +159,13 @@ class ProcessorTracker : public ProcessorBase * * The ProcessorTracker only processes incoming captures (it is not called). */ - void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {}; + void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {} /** \brief trigger in capture * * Returns true if processCapture() should be called after the provided capture arrived. */ - bool triggerInCapture(CaptureBasePtr) const override; + bool triggerInCapture(CaptureBasePtr) const override { return true;} /** \brief trigger in key-frame * @@ -177,13 +177,13 @@ class ProcessorTracker : public ProcessorBase * * Returns true if the key frame should be stored */ - bool storeKeyFrame(FrameBasePtr) override; + bool storeKeyFrame(FrameBasePtr) override { return true;} /** \brief store capture * * Returns true if the capture should be stored */ - bool storeCapture(CaptureBasePtr) override; + bool storeCapture(CaptureBasePtr) override { return false;} /** Pre-process incoming Capture * diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 8422be1cbeb5a4fb49d86aa40f6b2688460caf43..0627215bf19e5d8a50f7728e4c8ae376f320f77f 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -66,7 +66,7 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) : hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>()), map_ptr_(std::make_shared<MapBase>()), - processor_is_motion_map_(), + motion_provider_map_(), frame_structure_(_frame_structure), prior_options_(std::make_shared<PriorOptions>()) { @@ -442,7 +442,7 @@ TimeStamp Problem::getTimeStamp ( ) const { TimeStamp ts = TimeStamp::Invalid(); - for (const auto& prc_pair : processor_is_motion_map_) + for (const auto& prc_pair : motion_provider_map_) if (prc_pair.second->getTimeStamp().ok()) if ( (not ts.ok() ) or prc_pair.second->getTimeStamp() > ts) ts = prc_pair.second->getTimeStamp(); @@ -469,8 +469,8 @@ VectorComposite Problem::getState(const StateStructure& _structure) const VectorComposite state; - // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state - for (const auto& prc_pair : processor_is_motion_map_) + // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state + for (const auto& prc_pair : motion_provider_map_) { const auto& prc_state = prc_pair.second->getState(structure); @@ -517,8 +517,8 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ VectorComposite state; - // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state - for (const auto& prc_pair : processor_is_motion_map_) + // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state + for (const auto& prc_pair : motion_provider_map_) { const auto& prc_state = prc_pair.second->getState(_ts, structure); @@ -565,8 +565,8 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const VectorComposite odom_state; - // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state - for (const auto& prc_pair : processor_is_motion_map_) + // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state + for (const auto& prc_pair : motion_provider_map_) { const auto& prc_state = prc_pair.second->getOdometry(); @@ -628,39 +628,39 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm) } -void Problem::addProcessorIsMotion(IsMotionPtr _is_motion_ptr) +void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr) { // Check if is state getter - if (not _is_motion_ptr->isStateGetter()) + if (not _motion_provider_ptr->isStateGetter()) { - WOLF_WARN("Problem::addProcessorIsMotion: adding a IsMotion processor with state_getter=false. Not adding this processor"); + WOLF_WARN("Problem::addMotionProvider: adding a MotionProvider processor with state_getter=false. Not adding this processor"); return; } // check duplicated priority - while (processor_is_motion_map_.count(_is_motion_ptr->getStatePriority()) == 1) + while (motion_provider_map_.count(_motion_provider_ptr->getStatePriority()) == 1) { - WOLF_ERROR("Problem::addProcessorIsMotion: adding a IsMotion processor with priority = ", - _is_motion_ptr->getStatePriority(), + WOLF_ERROR("Problem::addMotionProvider: adding a MotionProvider processor with priority = ", + _motion_provider_ptr->getStatePriority(), " which is already taken. Trying to add it with priority = ", - _is_motion_ptr->getStatePriority()+1); - _is_motion_ptr->setStatePriority(_is_motion_ptr->getStatePriority()+1); + _motion_provider_ptr->getStatePriority()+1); + _motion_provider_ptr->setStatePriority(_motion_provider_ptr->getStatePriority()+1); } // add to map ordered by priority - processor_is_motion_map_.emplace(_is_motion_ptr->getStatePriority(), _is_motion_ptr); - appendToStructure(_is_motion_ptr->getStateStructure()); + motion_provider_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr); + appendToStructure(_motion_provider_ptr->getStateStructure()); } -void Problem::removeProcessorIsMotion(IsMotionPtr proc) +void Problem::removeMotionProvider(MotionProviderPtr proc) { - WOLF_WARN_COND(processor_is_motion_map_.count(proc->getStatePriority()) == 0, "Problem::clearProcessorIsMotion: missing processor"); + WOLF_WARN_COND(motion_provider_map_.count(proc->getStatePriority()) == 0, "Problem::clearMotionProvider: missing processor"); - processor_is_motion_map_.erase(proc->getStatePriority()); + motion_provider_map_.erase(proc->getStatePriority()); // rebuild frame structure with remaining motion processors frame_structure_.clear(); - for (const auto& pm : processor_is_motion_map_) + for (const auto& pm : motion_provider_map_) appendToStructure(pm.second->getStateStructure()); } @@ -698,7 +698,7 @@ bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance) { - WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); + WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotionProvider() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); // pause processor profiling @@ -1088,7 +1088,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts) prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state); // Update origin for odometry processors - for (auto proc_pair : processor_is_motion_map_) + for (auto proc_pair : motion_provider_map_) proc_pair.second->setOdometry(prior_options_->state); if (prior_options_->mode == "fix") diff --git a/src/processor/is_motion.cpp b/src/processor/motion_provider.cpp similarity index 71% rename from src/processor/is_motion.cpp rename to src/processor/motion_provider.cpp index 9ec0b07307408cf0aa3fc0241d25907d1eed8a7f..2ffbe2d2b9032621cc848b9531fa487b9fa791f7 100644 --- a/src/processor/is_motion.cpp +++ b/src/processor/motion_provider.cpp @@ -19,25 +19,25 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -#include "core/processor/is_motion.h" +#include <core/processor/motion_provider.h> #include "core/problem/problem.h" using namespace wolf; -void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr) +void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr) { setOdometry(_prb_ptr->stateZero(state_structure_)); if (not isStateGetter()) { - WOLF_WARN("IsMotion::addToProblem: IsMotion processor with state_getter=false. Not adding this processor"); + WOLF_WARN("MotionProvider::addToProblem: MotionProvider processor with state_getter=false. Not adding this processor"); return; } - _prb_ptr->addProcessorIsMotion(_motion_ptr); + _prb_ptr->addMotionProvider(_motion_ptr); } -void IsMotion::setOdometry(const VectorComposite& _odom) +void MotionProvider::setOdometry(const VectorComposite& _odom) { - assert(_odom.includesStructure(state_structure_) && "IsMotion::setOdometry(): any key missing in _odom."); + assert(_odom.includesStructure(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom."); for (auto key : state_structure_) odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_ diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 5046d2a6be1c6e4540bc22031daec8666d4f2f2c..0e88a448e98041ce034390d8bfd324f41528b1d6 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -57,17 +57,17 @@ bool ProcessorBase::permittedKeyFrame() void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) { assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame"); - WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp()); + WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp()); // profiling n_kf_callback_++; startKFProfiling(); - // asking if key frame should be stored + // asking if frame should be stored if (storeKeyFrame(_keyframe_ptr)) buffer_pack_kf_.add(_keyframe_ptr, _time_tol_other); - // if trigger true -> processKeyFrame + // asking if frame should be processed if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other)) processKeyFrame(_keyframe_ptr, _time_tol_other); @@ -78,7 +78,7 @@ void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _ 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()); + WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp()); // profiling n_capture_callback_++; @@ -92,7 +92,7 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr) if (storeCapture(_capture_ptr)) buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr); - // if trigger, process directly without buffering + // asking if capture should be processed if (triggerInCapture(_capture_ptr)) processCapture(_capture_ptr); @@ -107,12 +107,12 @@ void ProcessorBase::remove() is_removing_ = true; ProcessorBasePtr this_p = shared_from_this(); - if (isMotion()) + if (isMotionProvider()) { ProblemPtr P = getProblem(); - auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() ); + auto this_proc_cast_attempt = std::dynamic_pointer_cast<MotionProvider>( shared_from_this() ); if(P && this_proc_cast_attempt ) - P->removeProcessorIsMotion(this_proc_cast_attempt); + P->removeMotionProvider(this_proc_cast_attempt); } // remove from upstream @@ -150,9 +150,9 @@ void ProcessorBase::setProblem(ProblemPtr _problem) NodeBase::setProblem(_problem); // adding processor is motion to the processor is motion vector - auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this()); - if (is_motion_ptr) - is_motion_ptr->addToProblem(_problem, is_motion_ptr); + auto motion_provider_ptr = std::dynamic_pointer_cast<MotionProvider>(shared_from_this()); + if (motion_provider_ptr) + motion_provider_ptr->addToProblem(_problem, motion_provider_ptr); } ///////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp index d818ee50d502ca1751c3a06a03ac7d5553bcc7ef..927d873f48fe70ffaef085a0d28a9fdf84bbf03f 100644 --- a/src/processor/processor_loop_closure.cpp +++ b/src/processor/processor_loop_closure.cpp @@ -82,7 +82,7 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t { /* This function has 4 scenarios: * 1. Frame already have a capture of the sensor -> process - * 2. Frame has a timestamp within time tolerances of any stored capture -> link + process + * 2. Frame has a timestamp within time tolerances of some stored capture -> link + process * 3. Frame is more recent than any stored capture -> store frame to be processed later in processCapture * 4. Otherwise: The frame is not compatible with any stored capture -> discard frame */ diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 9b80af2d981cf234fd2539ce2f5f85f9f335ebb6..6b9b4b031b1091ca2e832f3f76336fd7cffc943c 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -44,7 +44,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type, SizeEigen _calib_size, ParamsProcessorMotionPtr _params_motion) : ProcessorBase(_type, _dim, _params_motion), - IsMotion(_state_structure, _params_motion), + MotionProvider(_state_structure, _params_motion), params_motion_(_params_motion), processing_step_(RUNNING_WITHOUT_KF), x_size_(_state_size), @@ -999,15 +999,6 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep() return nullptr; } -bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr) -{ - return true; -} -bool ProcessorMotion::storeCapture(CaptureBasePtr _cap_ptr) -{ - return false; -} - TimeStamp ProcessorMotion::getTimeStamp ( ) const { if (not origin_ptr_ or diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index b9fd36ec444a345ad57ffc4ea3e81d74007a5911..961937caf44a126aebcf91e5586e3f07c0e3e979 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -218,7 +218,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr) establishFactors(); // Call the new keyframe callback in order to let the other processors to establish their factors - getProblem()->keyFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance); + getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this(), params_tracker_->time_tolerance); // Update pointers resetDerived(); @@ -315,19 +315,6 @@ void ProcessorTracker::computeProcessingStep() } } -bool ProcessorTracker::triggerInCapture(CaptureBasePtr _cap_ptr) const -{ - return true; -} -bool ProcessorTracker::storeKeyFrame(FrameBasePtr _frame_ptr) -{ - return true; -} -bool ProcessorTracker::storeCapture(CaptureBasePtr _cap_ptr) -{ - return false; -} - void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const { _stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl; diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp index 185e0eb5ba6c3f0a87a5cf4f913047388439a335..760497fb3c58121df45e64f106a8adfeababf746 100644 --- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp +++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp @@ -52,14 +52,14 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame) params_swdr_->n_frames_recent - 1)->second; // compose motion captures for all processors motion - for (auto is_motion_pair : getProblem()->getProcessorIsMotionMap()) + for (auto motion_provider_pair : getProblem()->getMotionProviderMap()) { - auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion_pair.second); + auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(motion_provider_pair.second); if (proc_motion == nullptr) { - // FIXME: IsMotion::mergeCaptures pure virtual in IsMotion without need of casting - WOLF_INFO("TreeManagerSlidingWindowDualRate::keyFrameCallback: IsMotion ", - std::dynamic_pointer_cast<ProcessorBase>(is_motion_pair.second)->getName(), + // FIXME: MotionProvider::mergeCaptures pure virtual in MotionProvider without need of casting + WOLF_INFO("TreeManagerSlidingWindowDualRate::keyFrameCallback: MotionProvider ", + std::dynamic_pointer_cast<ProcessorBase>(motion_provider_pair.second)->getName(), " couldn't be casted to ProcessorMotion. Not merging its captures..."); continue; } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 46964ac57b3430b8113edbb7ee23d4923fc0d50b..0615b91df543890745e5aeb9f5f687aaead81ae8 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -89,8 +89,8 @@ wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp) target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME}) # IsMotion classes test -wolf_add_gtest(gtest_is_motion gtest_is_motion.cpp) -target_link_libraries(gtest_is_motion ${PLUGIN_NAME}) +wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp) +target_link_libraries(gtest_motion_provider ${PLUGIN_NAME}) # LocalParametrizationXxx classes test wolf_add_gtest(gtest_local_param gtest_local_param.cpp) diff --git a/test/dummy/processor_is_motion_dummy.h b/test/dummy/processor_motion_provider_dummy.h similarity index 68% rename from test/dummy/processor_is_motion_dummy.h rename to test/dummy/processor_motion_provider_dummy.h index 08ef74ff9689b2a9714f35226e1b7ff21bb319c8..7337132a0dc857a8a8948abee1b9ad29b4943fdc 100644 --- a/test/dummy/processor_is_motion_dummy.h +++ b/test/dummy/processor_motion_provider_dummy.h @@ -20,49 +20,49 @@ // //--------LICENSE_END-------- /* - * processor_is_motion_dummy.h + * processor_motion_provider_dummy.h * * Created on: Mar 8, 2021 * Author: joanvallve */ -#ifndef TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_ -#define TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_ +#ifndef TEST_DUMMY_PROCESSOR_MOTION_PROVIDER_DUMMY_H_ +#define TEST_DUMMY_PROCESSOR_MOTION_PROVIDER_DUMMY_H_ +#include <core/processor/motion_provider.h> #include "core/processor/processor_base.h" -#include "core/processor/is_motion.h" namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorIsMotionDummy); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProviderDummy); -struct ParamsProcessorIsMotionDummy : public ParamsProcessorBase, public ParamsIsMotion +struct ParamsMotionProviderDummy : public ParamsProcessorBase, public ParamsMotionProvider { std::string state_structure = "PO"; - ParamsProcessorIsMotionDummy() = default; - ParamsProcessorIsMotionDummy(std::string _unique_name, const ParamsServer& _server): + ParamsMotionProviderDummy() = default; + ParamsMotionProviderDummy(std::string _unique_name, const ParamsServer& _server): ParamsProcessorBase(_unique_name, _server), - ParamsIsMotion(_unique_name, _server) + ParamsMotionProvider(_unique_name, _server) { }; }; -WOLF_PTR_TYPEDEFS(ProcessorIsMotionDummy); +WOLF_PTR_TYPEDEFS(MotionProviderDummy); -class ProcessorIsMotionDummy : public ProcessorBase, public IsMotion +class MotionProviderDummy : public ProcessorBase, public MotionProvider { public: - ProcessorIsMotionDummy(ParamsProcessorIsMotionDummyPtr _params) : - ProcessorBase("ProcessorIsMotionDummy", 2, _params), - IsMotion(_params->state_structure, _params) + MotionProviderDummy(ParamsMotionProviderDummyPtr _params) : + ProcessorBase("MotionProviderDummy", 2, _params), + MotionProvider(_params->state_structure, _params) {} - ~ProcessorIsMotionDummy(){}; + ~MotionProviderDummy(){}; // Factory method for high level API - WOLF_PROCESSOR_CREATE(ProcessorIsMotionDummy, ParamsProcessorIsMotionDummy); + WOLF_PROCESSOR_CREATE(MotionProviderDummy, ParamsMotionProviderDummy); void configure(SensorBasePtr _sensor) override {}; void processCapture(CaptureBasePtr) override {}; @@ -90,8 +90,8 @@ class ProcessorIsMotionDummy : public ProcessorBase, public IsMotion // Register in the FactoryProcessor #include "core/processor/factory_processor.h" namespace wolf { -WOLF_REGISTER_PROCESSOR(ProcessorIsMotionDummy); -WOLF_REGISTER_PROCESSOR_AUTO(ProcessorIsMotionDummy); +WOLF_REGISTER_PROCESSOR(MotionProviderDummy); +WOLF_REGISTER_PROCESSOR_AUTO(MotionProviderDummy); } // namespace wolf -#endif /* TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_ */ +#endif /* TEST_DUMMY_PROCESSOR_MOTION_PROVIDER_DUMMY_H_ */ diff --git a/test/gtest_is_motion.cpp b/test/gtest_motion_provider.cpp similarity index 82% rename from test/gtest_is_motion.cpp rename to test/gtest_motion_provider.cpp index 222933730d1cece920f68e911301c35cf539e2bc..a4a76f7b64f4bc9ea8019f5c63dc183759d73b1f 100644 --- a/test/gtest_is_motion.cpp +++ b/test/gtest_motion_provider.cpp @@ -20,10 +20,11 @@ // //--------LICENSE_END-------- //Wolf +#include "dummy/processor_motion_provider_dummy.h" + +#include "core/processor/motion_provider.h" #include "core/utils/utils_gtest.h" -#include "core/processor/is_motion.h" -#include "dummy/processor_is_motion_dummy.h" #include "core/sensor/sensor_odom_2d.h" #include "core/problem/problem.h" @@ -36,13 +37,13 @@ using namespace wolf; using namespace Eigen; -class IsMotionTest : public testing::Test +class MotionProviderTest : public testing::Test { public: ProblemPtr problem; SensorBasePtr sen; ProcessorBasePtr prc1, prc2, prc3; - IsMotionPtr im1, im2, im3; + MotionProviderPtr im1, im2, im3; std::string wolf_root = _WOLF_ROOT_DIR; double dt = 0.01; @@ -59,37 +60,37 @@ class IsMotionTest : public testing::Test wolf_root + "/test/yaml/sensor_odom_2d.yaml"); // Install 3 odom processors - ParamsProcessorIsMotionDummyPtr prc1_params = std::make_shared<ParamsProcessorIsMotionDummy>(); + ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>(); prc1_params->time_tolerance = dt/2; prc1_params->state_structure = "PO"; prc1_params->state_getter = false; - prc1 = problem->installProcessor("ProcessorIsMotionDummy", + prc1 = problem->installProcessor("MotionProviderDummy", "not getter processor", sen, prc1_params); - im1 = std::dynamic_pointer_cast<IsMotion>(prc1); + im1 = std::dynamic_pointer_cast<MotionProvider>(prc1); - ParamsProcessorIsMotionDummyPtr prc2_params = std::make_shared<ParamsProcessorIsMotionDummy>(); + ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>(); prc2_params->time_tolerance = dt/2; prc2_params->state_structure = "PO"; prc2_params->state_getter = true; prc2_params->state_priority = 1; - prc2 = problem->installProcessor("ProcessorIsMotionDummy", + prc2 = problem->installProcessor("MotionProviderDummy", "getter processor", sen, prc2_params); - im2 = std::dynamic_pointer_cast<IsMotion>(prc2); + im2 = std::dynamic_pointer_cast<MotionProvider>(prc2); - ParamsProcessorIsMotionDummyPtr prc3_params = std::make_shared<ParamsProcessorIsMotionDummy>(); + ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>(); prc3_params->time_tolerance = dt/2; prc3_params->state_structure = "POV"; prc3_params->state_getter = true; prc3_params->state_priority = 1; - prc3 = problem->installProcessor("ProcessorIsMotionDummy", + prc3 = problem->installProcessor("MotionProviderDummy", "getter processor low priority", sen, prc3_params); - im3 = std::dynamic_pointer_cast<IsMotion>(prc3); + im3 = std::dynamic_pointer_cast<MotionProvider>(prc3); } }; @@ -103,12 +104,12 @@ class IsMotionTest : public testing::Test * - Problem::getState/getOdometry (state_getter, state_priority) */ -TEST_F(IsMotionTest, install) +TEST_F(MotionProviderTest, install) { - // All isMotion() = true - ASSERT_TRUE (prc1->isMotion()); - ASSERT_TRUE (prc2->isMotion()); - ASSERT_TRUE (prc3->isMotion()); + // All MotionProvider() = true + ASSERT_TRUE (prc1->isMotionProvider()); + ASSERT_TRUE (prc2->isMotionProvider()); + ASSERT_TRUE (prc3->isMotionProvider()); ASSERT_TRUE (im1 != nullptr); ASSERT_TRUE (im2 != nullptr); ASSERT_TRUE (im3 != nullptr); @@ -123,13 +124,13 @@ TEST_F(IsMotionTest, install) ASSERT_EQ(im2->getStateStructure(), "PO"); ASSERT_EQ(im3->getStateStructure(), "POV"); - // Only 2 and 3 in problem::processor_is_motion_map_ - ASSERT_EQ(problem->getProcessorIsMotionMap().size(), 2); - ASSERT_EQ(problem->getProcessorIsMotionMap().begin()->second, im2); - ASSERT_EQ(std::next(problem->getProcessorIsMotionMap().begin())->second, im3); + // Only 2 and 3 in problem::motion_provider_map_ + ASSERT_EQ(problem->getMotionProviderMap().size(), 2); + ASSERT_EQ(problem->getMotionProviderMap().begin()->second, im2); + ASSERT_EQ(std::next(problem->getMotionProviderMap().begin())->second, im3); } -TEST_F(IsMotionTest, odometry) +TEST_F(MotionProviderTest, odometry) { VectorComposite odom_p("P"); // missing P key odom_p['P'] = Vector2d::Zero(); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index e39eaf59ab679dea7c5d6b333e1be69dbea5f6c7..461e7c130cc1dba7a08d9b0cadffaf7c0ea7bc7b 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -85,7 +85,7 @@ TEST(Problem, Processor) ProblemPtr P = Problem::create("PO", 3); // check motion processor is null - ASSERT_FALSE(P->getProcessorIsMotion()); + ASSERT_TRUE(P->getMotionProviderMap().empty()); // add a motion sensor and processor auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d()); @@ -94,7 +94,7 @@ TEST(Problem, Processor) auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>()); // check motion processor IS NOT by emplace - ASSERT_EQ(P->getProcessorIsMotion(), Pm); + ASSERT_EQ(P->getMotionProviderMap().begin()->second, Pm); } TEST(Problem, Installers) @@ -109,16 +109,16 @@ TEST(Problem, Installers) auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer"); // check motion processor IS NOT set - ASSERT_FALSE(P->getProcessorIsMotion()); + ASSERT_TRUE(P->getMotionProviderMap().empty()); // 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->getProcessorIsMotion() != nullptr); + ASSERT_FALSE(P->getMotionProviderMap().empty()); // check motion processor is correct - ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorIsMotion()), pm); + ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getMotionProviderMap().begin()->second), pm); } TEST(Problem, SetOrigin_PO_2d) diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index c2111b31aea173b2e5efa3433ce2cf927d5eeadc..52aa50d70997a4d34d1d5d0ce652b34ecf9008cf 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -44,7 +44,7 @@ using namespace wolf; using namespace Eigen; -TEST(ProcessorBase, IsMotion) +TEST(ProcessorBase, MotionProvider) { using namespace wolf; using std::shared_ptr; @@ -80,8 +80,8 @@ TEST(ProcessorBase, IsMotion) sens_odo, proc_odo_params); - ASSERT_FALSE(proc_trk->isMotion()); - ASSERT_TRUE (proc_odo->isMotion()); + ASSERT_FALSE(proc_trk->isMotionProvider()); + ASSERT_TRUE (proc_odo->isMotionProvider()); }