From 7f8c1d1ba9fc1e49d09c11b20868b2eb2f54e7ab Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 24 May 2019 16:30:04 +0200
Subject: [PATCH] setProcessorMotion machinery protected and moved to
ProcessorMotion::setProblem
---
include/core/problem/problem.h | 20 ++++++++++++--------
include/core/processor/processor_motion.h | 2 ++
src/problem/problem.cpp | 9 ---------
src/processor/processor_motion.cpp | 16 ++++++++++++++++
4 files changed, 30 insertions(+), 17 deletions(-)
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 6b2e32db1..ba7e88eb6 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -38,6 +38,7 @@ enum Notification
class Problem : public std::enable_shared_from_this<Problem>
{
friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
+ friend ProcessorMotion;
protected:
HardwareBasePtr hardware_ptr_;
@@ -134,20 +135,23 @@ class Problem : public std::enable_shared_from_this<Problem>
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename = "");
- /**
- Custom installProcessor to be used with parameters server
- */
- ProcessorBasePtr installProcessor(const std::string& _prc_type, //
- const std::string& _unique_processor_name, //
- const std::string& _corresponding_sensor_name, //
- const paramsServer& _server);
- /** \brief Set the processor motion
+ /**
+ Custom installProcessor to be used with parameters server
+ */
+ ProcessorBasePtr installProcessor(const std::string& _prc_type, //
+ const std::string& _unique_processor_name, //
+ const std::string& _corresponding_sensor_name, //
+ const paramsServer& _server);
+ protected:
+ /** \brief Set the processor motion
*
* Set the processor motion.
*/
void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
void clearProcessorMotion();
+
+ public:
ProcessorMotionPtr& getProcessorMotion();
// Trajectory branch ----------------------------------
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 5b971769b..0c6f2175e 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -209,6 +209,8 @@ class ProcessorMotion : public ProcessorBase
*/
FrameBasePtr setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
+ virtual void setProblem(ProblemPtr);
+
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 62c035cf0..87819e750 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -163,17 +163,8 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _prc_params, _corresponding_sensor_ptr);
prc_ptr->configure(_corresponding_sensor_ptr);
- // _corresponding_sensor_ptr->addProcessor(prc_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
- // setting the origin in all processor motion if origin already setted
- if (prc_ptr->isMotion() && prior_is_set_)
- (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
-
- // setting the main processor motion
- if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
- processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
-
return prc_ptr;
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index bb2812f75..a24cd7ed2 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -681,4 +681,20 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
return pack;
}
+void ProcessorMotion::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+
+ // set the origin
+ if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr)
+ this->setOrigin(this->getProblem()->getLastKeyFrame());
+
+ // set the main processor motion
+ if (this->getProblem()->getProcessorMotion() == nullptr)
+ this->getProblem()->setProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this()));
+};
+
}
--
GitLab