Implement PM::getState(structure)

src/processor/processor_motion.cpp

@@ -452,7 +452,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
      */
 
     // Get state of origin
-    const auto& x_origin = getOrigin()->getFrame()->getState();
+    const auto& x_origin = getOrigin()->getFrame()->getState(state_structure_);
 
     // Get most rescent motion
     const auto& motion = last_ptr_->getBuffer().back();
@@ -486,7 +486,19 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
         statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
     }
 
-    return state;
+    if (_structure == "")
+        return state;
+
+    else
+    {
+        // remove states not requested
+        for (const auto& pair_key_vec : state)
+        {
+            if (_structure.find(pair_key_vec.first) == std::string::npos)
+                state.erase(pair_key_vec.first);
+        }
+        return state;
+    }
 }
 
 
@@ -529,7 +541,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
 
         // Get state of origin
         CaptureBasePtr cap_orig   = capture_motion->getOriginCapture();
-        const auto& x_origin = cap_orig->getFrame()->getState();
+        const auto& x_origin = cap_orig->getFrame()->getState(state_structure_);
 
         // Get motion at time stamp
         const auto& motion = capture_motion->getBuffer().getMotion(_ts);
@@ -564,8 +576,19 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
             statePlusDelta(x_origin, delta_preint, _ts - cap_orig->getTimeStamp(), state);
         }
 
-        // return success
-        return state;
+        if (_structure == "")
+            return state;
+
+        else
+        {
+            // remove states not requested
+            for (const auto& pair_key_vec : state)
+            {
+                if (_structure.find(pair_key_vec.first) == std::string::npos)
+                    state.erase(pair_key_vec.first);
+            }
+            return state;
+        }
 
     }
 }