implemented and gtests fixed

include/core/problem/problem.h

@@ -58,7 +58,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         HardwareBasePtr     hardware_ptr_;
         TrajectoryBasePtr   trajectory_ptr_;
         MapBasePtr          map_ptr_;
-        std::list<IsMotionPtr>  processor_is_motion_list_;
+        std::map<int, IsMotionPtr>  processor_is_motion_map_;
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
         SizeEigen state_size_, state_cov_size_, dim_;
         std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -178,7 +178,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
     public:
         IsMotionPtr getProcessorIsMotion();
-        std::list<IsMotionPtr> getProcessorIsMotionList();
+        std::map<int,IsMotionPtr> getProcessorIsMotionMap();
         
 
 
@@ -431,15 +431,15 @@ inline bool Problem::isPriorSet() const
 
 inline IsMotionPtr Problem::getProcessorIsMotion()
 {
-    if (!processor_is_motion_list_.empty())
-        return processor_is_motion_list_.front();
+    if (not processor_is_motion_map_.empty())
+        return processor_is_motion_map_.begin()->second;
     return nullptr;
 }
 
 
-inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
+inline std::map<int,IsMotionPtr> Problem::getProcessorIsMotionMap()
 {
-    return processor_is_motion_list_;
+    return processor_is_motion_map_;
 }
 
 inline SizeStd Problem::getStateBlockNotificationMapSize() const

include/core/processor/is_motion.h

@@ -10,10 +10,34 @@
 
 #include "core/common/wolf.h"
 #include "core/state_block/state_composite.h"
+#include "core/utils/params_server.h"
 
 namespace wolf
 {
 
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsIsMotion);
+
+struct ParamsIsMotion
+{
+    std::string name;
+    bool state_getter = true;
+    int state_priority = 1;
+
+    ParamsIsMotion() = default;
+    ParamsIsMotion(std::string _unique_name, const ParamsServer& _server)
+    {
+        name            = _server.getParam<std::string>("processor/" + _unique_name + "/name");
+        state_getter    = _server.getParam<bool>("processor/" + _unique_name + "/state_getter");
+        state_priority  = _server.getParam<double>("processor/" + _unique_name + "/state_priority");
+    }
+    std::string print() const
+    {
+      return  "name: "           + name                           + "\n"
+            + "state_getter: "   + std::to_string(state_getter)   + "\n"
+            + "state_priority: " + std::to_string(state_priority) + "\n";
+    }
+
+};
 class TimeStamp;
 
 WOLF_PTR_TYPEDEFS(IsMotion);
@@ -22,7 +46,7 @@ class IsMotion
 {
     public:
 
-        IsMotion(const StateStructure& _structure);
+        IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params);
         virtual ~IsMotion();
 
         // Queries to the processor:
@@ -31,6 +55,12 @@ class IsMotion
         virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
 
         VectorComposite getOdometry ( ) const;
+
+        std::string getProcessorName() const;
+        bool isStateGetter() const;
+        int getStatePriority() const;
+        void setStatePriority(int);
+
     private:
         void setOdometry(const VectorComposite& _zero_odom) {odometry_ = _zero_odom;}
 
@@ -42,11 +72,12 @@ class IsMotion
     protected:
         StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
         VectorComposite odometry_;
-
+        ParamsIsMotionPtr params_is_motion_;
 };
 
-inline IsMotion::IsMotion(const StateStructure& _structure) :
-        state_structure_(_structure)
+inline IsMotion::IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params) :
+        state_structure_(_structure),
+        params_is_motion_(_params)
 {
     //
 }
@@ -68,6 +99,26 @@ inline IsMotion::~IsMotion()
 {
 }
 
+inline std::string IsMotion::getProcessorName() const
+{
+    return params_is_motion_->name;
+}
+
+inline bool IsMotion::isStateGetter() const
+{
+    return params_is_motion_->state_getter;
+}
+
+inline int IsMotion::getStatePriority() const
+{
+    return params_is_motion_->state_priority;
+}
+
+inline void IsMotion::setStatePriority(int _priority)
+{
+    params_is_motion_->state_priority = _priority;
+}
+
 } /* namespace wolf */
 
 #endif /* PROCESSOR_IS_MOTION_H_ */

include/core/processor/processor_motion.h

@@ -23,7 +23,7 @@ namespace wolf
 
 WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorMotion);
 
-struct ParamsProcessorMotion : public ParamsProcessorBase
+struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion
 {
         double max_time_span    = 0.5;
         unsigned int max_buff_length  = 10;
@@ -33,7 +33,8 @@ struct ParamsProcessorMotion : public ParamsProcessorBase
 
         ParamsProcessorMotion() = default;
         ParamsProcessorMotion(std::string _unique_name, const ParamsServer& _server):
-            ParamsProcessorBase(_unique_name, _server)
+            ParamsProcessorBase(_unique_name, _server),
+            ParamsIsMotion(_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");
@@ -43,7 +44,8 @@ struct ParamsProcessorMotion : public ParamsProcessorBase
         }
         std::string print() const override
         {
-          return ParamsProcessorBase::print() + "\n"
+          return ParamsProcessorBase::print() + "\n" +
+                 ParamsIsMotion::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"

src/problem/problem.cpp

@@ -45,7 +45,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_list_(std::list<IsMotionPtr>()),
+        processor_is_motion_map_(),
         frame_structure_(_frame_structure),
         prior_options_(std::make_shared<PriorOptions>())
 {
@@ -420,10 +420,10 @@ TimeStamp Problem::getTimeStamp ( ) const
 {
     TimeStamp  ts = TimeStamp::Invalid();
 
-    for (const auto& prc : processor_is_motion_list_)
-        if (prc->getTimeStamp().ok())
-            if ( (not ts.ok() ) or prc->getTimeStamp() > ts)
-                ts = prc->getTimeStamp();
+    for (const auto& prc_pair : processor_is_motion_map_)
+        if (prc_pair.second->getTimeStamp().ok())
+            if ( (not ts.ok() ) or prc_pair.second->getTimeStamp() > ts)
+                ts = prc_pair.second->getTimeStamp();
 
 
     if (not ts.ok())
@@ -447,13 +447,15 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
 
     VectorComposite state;
 
-    // compose the states of all processor motions into one only state
-    for (const auto& prc : processor_is_motion_list_)
+    // 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_)
     {
-        const auto& prc_state = prc->getState(structure);
+        const auto& prc_state = prc_pair.second->getState(structure);
+
+        // transfer processor vector blocks to problem state
         for (const auto& pair_key_vec : prc_state)
         {
-            if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
+            if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
                 state.insert(pair_key_vec);
         }
     }
@@ -493,16 +495,15 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
 
     VectorComposite state;
 
-    for (const auto& prc : processor_is_motion_list_)
+    // 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_)
     {
-        const auto& prc_state = prc->getState(_ts, structure);
+        const auto& prc_state = prc_pair.second->getState(_ts, structure);
 
         // transfer processor vector blocks to problem state
         for (const auto& pair_key_vec : prc_state)
         {
-            const auto& key = pair_key_vec.first;
-
-            if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
+            if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
                 state.insert(pair_key_vec);
         }
     }
@@ -562,23 +563,40 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
 
 }
 
-void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr)
+void Problem::addProcessorIsMotion(IsMotionPtr _is_motion_ptr)
 {
-    processor_is_motion_list_.push_back(_processor_motion_ptr);
-    appendToStructure(_processor_motion_ptr->getStateStructure());
+    // Check if is state getter
+    if (not _is_motion_ptr->isStateGetter())
+    {
+        WOLF_WARN("Problem::addProcessorIsMotion: adding a IsMotion processor with state_getter=false. Not adding this processor");
+        return;
+    }
+
+    // check duplicated priority
+    while (processor_is_motion_map_.count(_is_motion_ptr->getStatePriority()) == 1)
+    {
+        WOLF_ERROR("Problem::addProcessorIsMotion: adding a IsMotion processor with priority = ",
+                   _is_motion_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);
+    }
+
+    // add to map ordered by priority
+    processor_is_motion_map_.emplace(_is_motion_ptr->getStatePriority(), _is_motion_ptr);
+    appendToStructure(_is_motion_ptr->getStateStructure());
 }
 
 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);
-    }
+    WOLF_WARN_COND(processor_is_motion_map_.count(proc->getStatePriority()) == 0, "Problem::clearProcessorIsMotion: missing processor");
+
+    processor_is_motion_map_.erase(proc->getStatePriority());
 
     // rebuild frame structure with remaining motion processors
     frame_structure_.clear();
-    for (const auto& pm : processor_is_motion_list_)
-        appendToStructure(pm->getStateStructure());
+    for (const auto& pm : processor_is_motion_map_)
+        appendToStructure(pm.second->getStateStructure());
 }
 
 VectorComposite Problem::stateZero ( const StateStructure& _structure ) const

src/processor/is_motion.cpp

@@ -5,6 +5,11 @@ using namespace wolf;
 
 void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
 {
+    if (not isStateGetter())
+    {
+        WOLF_WARN("IsMotion::addToProblem: IsMotion processor with state_getter=false. Not adding this processor");
+        return;
+    }
     setOdometry(_prb_ptr->stateZero(state_structure_));
     _prb_ptr->addProcessorIsMotion(_motion_ptr);
 }

src/processor/processor_motion.cpp

@@ -23,7 +23,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
                                  SizeEigen _calib_size,
                                  ParamsProcessorMotionPtr _params_motion) :
         ProcessorBase(_type, _dim, _params_motion),
-        IsMotion(_state_structure),
+        IsMotion(_state_structure, _params_motion),
         params_motion_(_params_motion),
         processing_step_(RUNNING_WITHOUT_KF),
         x_size_(_state_size),

src/tree_manager/tree_manager_sliding_window_dual_rate.cpp

@@ -31,11 +31,17 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
                                                         params_swdr_->n_frames_recent - 1)->second;
 
         // compose motion captures for all processors motion
-        for (auto is_motion : getProblem()->getProcessorIsMotionList())
+        for (auto is_motion_pair : getProblem()->getProcessorIsMotionMap())
         {
-            auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion);
+            auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion_pair.second);
             if (proc_motion == nullptr)
+            {
+                // FIXME: IsMotion::mergeCaptures pure virtual in IsMotion without need of casting
+                WOLF_INFO("TreeManagerSlidingWindowDualRate::keyFrameCallback: IsMotion ",
+                          is_motion_pair.second->getProcessorName(),
+                          " couldn't be casted to ProcessorMotion. Not merging its captures...");
                 continue;
+            }
 
             auto cap_prev = std::static_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor()));
             auto cap_next = std::static_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor()));

test/yaml/params_problem_odom_3d.yaml

@@ -41,3 +41,6 @@ config:
         angle_turned:           999   # radians (1 rad approx 57 deg, approx 60 deg)
       
       unmeasured_perturbation_std: 0.00111
+      
+      state_getter: true
+      state_priority: 1

test/yaml/params_tree_manager1.yaml

@@ -46,3 +46,7 @@ config:
         angle_turned:           0.1   # radians (1 rad approx 57 deg, approx 60 deg)
       
       unmeasured_perturbation_std: 0.00111
+      
+      state_getter: true
+      state_priority: 1
+      

test/yaml/params_tree_manager2.yaml

@@ -45,3 +45,7 @@ config:
         angle_turned:           0.1   # radians (1 rad approx 57 deg, approx 60 deg)
       
       unmeasured_perturbation_std: 0.00111
+      
+      state_getter: true
+      state_priority: 1
+      

test/yaml/params_tree_manager_sliding_window1.yaml

@@ -44,3 +44,7 @@ config:
         angle_turned:           0.1   # radians (1 rad approx 57 deg, approx 60 deg)
       
       unmeasured_perturbation_std: 0.00111
+      
+      state_getter: true
+      state_priority: 1
+      

test/yaml/params_tree_manager_sliding_window2.yaml

@@ -44,3 +44,7 @@ config:
         angle_turned:           0.1   # radians (1 rad approx 57 deg, approx 60 deg)
       
       unmeasured_perturbation_std: 0.00111
+      
+      state_getter: true
+      state_priority: 1
+      

test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml

@@ -52,3 +52,7 @@ config:
         angle_turned:           3.1   # radians (1 rad approx 57 deg, approx 60 deg)
       
       unmeasured_perturbation_std: 0.00111
+      
+      state_getter: true
+      state_priority: 1
+      

test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml

@@ -46,3 +46,6 @@ config:
         angle_turned:           3.1   # radians (1 rad approx 57 deg, approx 60 deg)
       
       unmeasured_perturbation_std: 0.00111
+      
+      state_getter: true
+      state_priority: 1