From 9a520e9bdb78aaa558d9f8ec8935e8ca0c970d03 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 4 Apr 2020 18:44:26 +0200
Subject: [PATCH] adding getStateComposition here and there

---
 include/core/problem/problem.h            | 10 ++++++---
 include/core/processor/is_motion.h        | 25 ++++++++++++++++++++---
 include/core/processor/processor_motion.h | 11 ++++++++++
 src/problem/problem.cpp                   |  2 +-
 4 files changed, 41 insertions(+), 7 deletions(-)

diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 219668403..f92d39c8b 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -42,6 +42,13 @@ class Problem : public std::enable_shared_from_this<Problem>
     friend ProcessorBase;
     friend ProcessorMotion;
 
+    // TODO move somewhere below
+    public:
+        StateComposite  getStateComposite() const;
+        StateComposite  getStateComposite(const TimeStamp& _ts) const;
+
+
+
     protected:
         TreeManagerBasePtr tree_manager_;
         HardwareBasePtr     hardware_ptr_;
@@ -279,9 +286,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Perturb state
         void            perturb(double amplitude = 0.01);
 
-        const StateComposite&  getStateComposite() const;
-        const StateComposite&  getStateComposite(const TimeStamp& _ts) const;
-
         // Map branch -----------------------------------------
         MapBasePtr getMap() const;
         void loadMap(const std::string& _filename_dot_yaml);
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 8655f4d78..25ded4c22 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -22,6 +22,14 @@ class IsMotion
 {
     public:
 
+        // TODO move somewhere below
+        virtual bool  getStateComposite(StateComposite& _state) const = 0;
+        virtual bool  getStateComposite(const TimeStamp& _ts, StateComposite& _state) const = 0;
+
+        StateComposite  getStateComposite() const;
+        StateComposite  getStateComposite(const TimeStamp& _ts) const;
+
+
         virtual ~IsMotion();
 
         // Queries to the processor:
@@ -44,9 +52,6 @@ class IsMotion
         TimeStamp       getCurrentTimeStamp() const;
         Eigen::VectorXd getState(const TimeStamp& _ts) const;
 
-        StateComposite  getStateComposite() const;
-        StateComposite  getStateComposite(const TimeStamp& _ts) const;
-
         const StateStructure& getStateStructure(){return state_structure_;};
         void setStateStructure(const StateStructure& _state_structure){state_structure_ = _state_structure;};
     
@@ -80,6 +85,20 @@ inline TimeStamp IsMotion::getCurrentTimeStamp() const
     return ts;
 }
 
+inline StateComposite IsMotion::getStateComposite() const
+{
+    StateComposite state;
+    getStateComposite(state);
+    return state;
+}
+
+inline StateComposite IsMotion::getStateComposite(const TimeStamp &_ts) const
+{
+    StateComposite state;
+    getStateComposite(_ts, state);
+    return state;
+}
+
 inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
 {
     Eigen::VectorXd x;
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 81a9184ad..e69d1f55b 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -133,6 +133,17 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
 class ProcessorMotion : public ProcessorBase, public IsMotion
 {
     public:
+
+
+
+        // TODO move somewhere below
+        virtual bool  getStateComposite(StateComposite& _state) const override;
+        virtual bool  getStateComposite(const TimeStamp& _ts, StateComposite& _state) const override;
+        using IsMotion::getStateComposite;
+
+
+
+
         typedef enum {
             RUNNING_WITHOUT_PACK,
             RUNNING_WITH_PACK_BEFORE_ORIGIN,
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 7788f8419..b7565f35c 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -376,7 +376,7 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c
         _state = zeroState();
 }
 
-inline const StateComposite& Problem::getStateComposite() const
+inline StateComposite Problem::getStateComposite() const
 {
     StateComposite state;
 
-- 
GitLab