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