From 14660fa6807cbfef4968d343ec215563766b170a Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Mon, 10 Oct 2016 16:57:30 +0200
Subject: [PATCH] enforce passing Capture by their Sensor
---
src/capture_base.cpp | 16 ++++++++--------
src/capture_base.h | 11 +++++++++--
src/capture_laser_2D.h | 8 ++++++++
src/examples/test_image.cpp | 3 ++-
src/examples/test_processor_image_landmark.cpp | 3 ++-
src/examples/test_processor_imu.cpp | 3 ++-
src/sensor_base.h | 15 +++++++++++++++
7 files changed, 46 insertions(+), 13 deletions(-)
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 1cceb9197..b36ad8625 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -42,14 +42,14 @@ void CaptureBase::destruct()
}
}
-void CaptureBase::process()
-{
- // Call all processors assigned to the sensor that captured this data
- for (auto processor_iter = sensor_ptr_->getProcessorListPtr()->begin(); processor_iter != sensor_ptr_->getProcessorListPtr()->end(); ++processor_iter)
- {
- (*processor_iter)->process(this);
- }
-}
+//void CaptureBase::process()
+//{
+// // Call all processors assigned to the sensor that captured this data
+// for (auto processor_iter = sensor_ptr_->getProcessorListPtr()->begin(); processor_iter != sensor_ptr_->getProcessorListPtr()->end(); ++processor_iter)
+// {
+// (*processor_iter)->process(this);
+// }
+//}
void CaptureBase::addFeatureList(FeatureBaseList& _new_ft_list)
{
diff --git a/src/capture_base.h b/src/capture_base.h
index 612941717..b9ef7848a 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -38,7 +38,7 @@ class CaptureBase : public NodeBase
public:
- CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
+ CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBasePtr _sensor_ptr = nullptr);
/** \brief Default destructor (not recommended)
*
@@ -67,12 +67,14 @@ class CaptureBase : public NodeBase
void getConstraintList(ConstraintBaseList& _ctr_list);
SensorBasePtr getSensorPtr() const;
+ virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
+
StateBlock* getSensorPPtr() const;
StateBlock* getSensorOPtr() const;
/** \brief Call all the processors for this Capture
*/
- virtual void process();
+ //virtual void process();
};
@@ -163,6 +165,11 @@ inline SensorBasePtr CaptureBase::getSensorPtr() const
return sensor_ptr_;
}
+inline void CaptureBase::setSensorPtr(const SensorBasePtr sensor_ptr)
+{
+ sensor_ptr_ = sensor_ptr;
+}
+
inline void CaptureBase::setTimeStamp(const TimeStamp& _ts)
{
time_stamp_ = _ts;
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 314ed3160..456d0f87d 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -30,6 +30,8 @@ class CaptureLaser2D : public CaptureBase
laserscanutils::LaserScan& getScan();
+ void setSensorPtr(const SensorBasePtr sensor_ptr);
+
private:
SensorLaser2D* laser_ptr_; //specific pointer to sensor laser 2D object
laserscanutils::LaserScan scan_;
@@ -41,6 +43,12 @@ inline laserscanutils::LaserScan& CaptureLaser2D::getScan()
return scan_;
}
+inline void CaptureLaser2D::setSensorPtr(const SensorBasePtr sensor_ptr)
+{
+ CaptureBase::setSensorPtr(sensor_ptr);
+ laser_ptr_ = (SensorLaser2D*)(sensor_ptr);
+}
+
} // namespace wolf
#endif /* CAPTURE_LASER_2D_H_ */
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index 0743e4dd7..8c5d9e78e 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -151,7 +151,8 @@ int main(int argc, char** argv)
// Preferred method with factory objects:
image_ptr = new CaptureImage(t, camera_ptr, frame[f % buffer_size]);
- image_ptr->process();
+ //image_ptr->process();
+ camera_ptr->addCapture(image_ptr);
//cv::imshow("test",frame[f % buffer_size]);
std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
cv::waitKey(5);
diff --git a/src/examples/test_processor_image_landmark.cpp b/src/examples/test_processor_image_landmark.cpp
index d72b8613a..c2340f760 100644
--- a/src/examples/test_processor_image_landmark.cpp
+++ b/src/examples/test_processor_image_landmark.cpp
@@ -177,7 +177,8 @@ int main(int argc, char** argv)
image_ptr = new CaptureImage(t, camera_ptr_, frame[f % buffer_size]);
/* process */
- image_ptr->process();
+ //image_ptr->process();
+ camera_ptr_->addCapture(image_ptr);
std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
cv::waitKey(0);
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index 6822ea3cb..f60fe8410 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -109,7 +109,8 @@ int main(int argc, char** argv)
imu_ptr->setTimeStamp(t);
// process data in capture
- imu_ptr ->process();
+ //imu_ptr ->process();
+ sensor_ptr->addCapture(imu_ptr);
#ifdef DEBUG_RESULTS
diff --git a/src/sensor_base.h b/src/sensor_base.h
index a08da78ea..8a398546b 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -130,10 +130,13 @@ class SensorBase : public NodeBase
ProblemPtr getProblem();
void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
+ bool addCapture(const CaptureBasePtr capture_ptr);
+
};
}
+#include "capture_base.h"
#include "processor_base.h"
#include "hardware_base.h"
@@ -216,6 +219,18 @@ inline Eigen::MatrixXs SensorBase::getNoiseCov()
return noise_cov_;
}
+inline bool SensorBase::addCapture(const CaptureBasePtr capture_ptr)
+{
+ capture_ptr->setSensorPtr(this);
+
+ for (const auto processor : processor_list_)
+ {
+ processor->process(capture_ptr);
+ }
+
+ return true;
+}
+
} // namespace wolf
#endif
--
GitLab