From c6bb755a4792d0b0c87ffb80cb6818629ad26101 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 11 Mar 2020 10:16:55 +0100
Subject: [PATCH] Use IsMotion in Problem
---
include/core/problem/problem.h | 12 ++++++-----
include/core/processor/is_motion.h | 8 +++-----
include/core/processor/processor_motion.h | 13 ++++++------
src/problem/problem.cpp | 25 ++++++++++++-----------
src/processor/processor_base.cpp | 2 +-
src/processor/processor_motion.cpp | 4 ++++
test/gtest_problem.cpp | 2 +-
7 files changed, 36 insertions(+), 30 deletions(-)
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 362235b95..877eacff4 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -21,6 +21,7 @@ struct ProcessorParamsBase;
#include "core/utils/params_server.hpp"
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
+#include "core/processor/is_motion.h"
// std includes
#include <mutex>
@@ -45,7 +46,8 @@ class Problem : public std::enable_shared_from_this<Problem>
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
- ProcessorMotionPtr processor_motion_ptr_;
+ IsMotionPtr processor_motion_ptr_;
+// IsMotionPtr is_motion_ptr_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -147,12 +149,12 @@ class Problem : public std::enable_shared_from_this<Problem>
*
* Set the processor motion.
*/
- void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
- ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
+ void setProcessorMotion(IsMotionPtr _processor_motion_ptr);
+ IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
void clearProcessorMotion();
public:
- ProcessorMotionPtr& getProcessorMotion();
+ IsMotionPtr& getProcessorMotion();
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
@@ -360,7 +362,7 @@ inline bool Problem::priorIsSet() const
return prior_is_set_;
}
-inline ProcessorMotionPtr& Problem::getProcessorMotion()
+inline IsMotionPtr& Problem::getProcessorMotion()
{
return processor_motion_ptr_;
}
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index 42139f0eb..410b6dd9e 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -15,10 +15,12 @@ namespace wolf
class TimeStamp;
+WOLF_PTR_TYPEDEFS(IsMotion);
+
class IsMotion
{
public:
- IsMotion();
+
virtual ~IsMotion();
// Queries to the processor:
@@ -57,10 +59,6 @@ class IsMotion
namespace wolf{
-inline IsMotion::IsMotion()
-{
-}
-
inline IsMotion::~IsMotion()
{
}
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index f3f0eff35..6e56fd99b 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -11,6 +11,7 @@
// Wolf
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
+#include "core/processor/is_motion.h"
#include "core/common/time_stamp.h"
#include "core/utils/params_server.hpp"
@@ -164,27 +165,27 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
/** \brief Fill a reference to the state integrated so far
* \param _x the returned state vector
*/
- void getCurrentState(Eigen::VectorXd& _x) const;
- void getCurrentTimeStamp(TimeStamp& _ts) const { _ts = getBuffer().get().back().ts_; }
+ virtual void getCurrentState(Eigen::VectorXd& _x) const override;
+ virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().get().back().ts_; }
/** \brief Get the state integrated so far
* \return the state vector
*/
- Eigen::VectorXd getCurrentState() const;
- TimeStamp getCurrentTimeStamp() const;
+ virtual Eigen::VectorXd getCurrentState() const override;
+ virtual TimeStamp getCurrentTimeStamp() const override;
/** \brief Fill the state corresponding to the provided time-stamp
* \param _ts the time stamp
* \param _x the returned state
* \return if state in the provided time-stamp could be resolved
*/
- bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const;
+ virtual bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const override;
/** \brief Get the state corresponding to the provided time-stamp
* \param _ts the time stamp
* \return the state vector
*/
- Eigen::VectorXd getState(const TimeStamp& _ts) const;
+ virtual Eigen::VectorXd getState(const TimeStamp& _ts) const override;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 7abc06284..1afab86df 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -291,7 +291,7 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
return (*sen_it);
}
-ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
+IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
{
for (auto sen : getHardware()->getSensorList()) // loop all sensors
{
@@ -322,7 +322,7 @@ ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_nam
return nullptr;
}
-void Problem::setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr)
+void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
{
processor_motion_ptr_ = _processor_motion_ptr;
}
@@ -495,16 +495,17 @@ bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
{
- if (_processor_ptr)
- {
- WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
- }
- else
- {
- WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
- }
-
- processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
+ // TODO
+// if (_processor_ptr)
+// {
+// WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+// }
+// else
+// {
+// WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
+// }
+//
+// processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
}
StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 39b77e59f..17bc8fffa 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -71,7 +71,7 @@ void ProcessorBase::remove()
if (isMotion())
{
ProblemPtr P = getProblem();
- if(P && P->getProcessorMotion()->id() == this->id())
+ if(P && this->isMotion() && P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>(shared_from_this()))
P->clearProcessorMotion();
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index c667b7193..73e63e5ef 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -1,4 +1,8 @@
+
+
#include "core/processor/processor_motion.h"
+
+
namespace wolf
{
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d39ce95f7..da8327290 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -122,7 +122,7 @@ TEST(Problem, Installers)
ASSERT_TRUE(P->getProcessorMotion());
// check motion processor is correct
- ASSERT_EQ(P->getProcessorMotion(), pm);
+ ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorMotion()), pm);
}
TEST(Problem, SetOrigin_PO_2D)
--
GitLab