From dc0a3dd68988b98e55df85671f3cffb8a3afa321 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Fri, 23 Jun 2023 12:37:15 +0200
Subject: [PATCH] WIP
---
demos/hello_wolf/capture_range_bearing.cpp | 6 -
demos/hello_wolf/feature_range_bearing.cpp | 6 -
demos/hello_wolf/processor_range_bearing.cpp | 6 -
demos/solver/test_SPQR.cpp | 6 -
demos/solver/test_ccolamd.cpp | 6 -
demos/solver/test_ccolamd_blocks.cpp | 6 -
demos/solver/test_iQR.cpp | 13 --
demos/solver/test_iQR_wolf.cpp | 6 -
demos/solver/test_iQR_wolf2.cpp | 6 -
.../test_incremental_ccolamd_blocks.cpp | 6 -
demos/solver/test_permutations.cpp | 6 -
include/core/problem/problem.h | 4 +-
include/core/processor/factory_processor.h | 11 +-
include/core/processor/motion_provider.h | 23 ---
include/core/processor/processor_base.h | 2 +-
src/capture/capture_odom_2d.cpp | 6 -
src/capture/capture_odom_3d.cpp | 6 -
src/ceres_wrapper/qr_manager.cpp | 6 -
src/feature/feature_motion.cpp | 6 -
src/problem/problem.cpp | 17 ++-
src/processor/processor_tracker.cpp | 6 -
src/processor/processor_tracker_feature.cpp | 6 -
test/CMakeLists.txt | 43 +++---
.../dummy/ProcessorMotionProviderDummy.schema | 2 +
.../ProcessorMotionProviderDummyPOV.schema | 2 +
test/dummy/processor_motion_provider_dummy.h | 12 +-
.../processor_motion_provider_dummy_pov.h | 70 +++++++++
test/gtest_emplace.cpp | 10 +-
test/gtest_factory_state_block.cpp | 6 -
test/gtest_feature_base.cpp | 6 -
test/gtest_frame_base.cpp | 6 +-
test/gtest_motion_buffer.cpp | 7 -
test/gtest_motion_provider.cpp | 139 ++++++++++--------
test/gtest_processor_base.cpp | 121 ++++++---------
test/gtest_schema.cpp | 2 +-
test/gtest_sensor_diff_drive.cpp | 66 +++++----
test/gtest_solver_manager.cpp | 6 -
test/gtest_solver_manager_multithread.cpp | 6 -
test/gtest_state_block.cpp | 6 -
test/gtest_trajectory.cpp | 6 -
.../processor_motion_provider_dummy1.yaml | 12 ++
.../processor_motion_provider_dummy2.yaml | 12 ++
.../processor_motion_provider_dummy3.yaml | 12 ++
test/yaml/processor_odom_2d.yaml | 16 +-
test/yaml/sensor_diff_drive.yaml | 1 +
45 files changed, 331 insertions(+), 404 deletions(-)
create mode 100644 test/dummy/ProcessorMotionProviderDummy.schema
create mode 100644 test/dummy/ProcessorMotionProviderDummyPOV.schema
create mode 100644 test/dummy/processor_motion_provider_dummy_pov.h
create mode 100644 test/yaml/processor_motion_provider_dummy1.yaml
create mode 100644 test/yaml/processor_motion_provider_dummy2.yaml
create mode 100644 test/yaml/processor_motion_provider_dummy3.yaml
diff --git a/demos/hello_wolf/capture_range_bearing.cpp b/demos/hello_wolf/capture_range_bearing.cpp
index cc627493a..ca2f7c10a 100644
--- a/demos/hello_wolf/capture_range_bearing.cpp
+++ b/demos/hello_wolf/capture_range_bearing.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * CaptureRangeBearing2d.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
#include "capture_range_bearing.h"
diff --git a/demos/hello_wolf/feature_range_bearing.cpp b/demos/hello_wolf/feature_range_bearing.cpp
index 73f1965f4..a99fa854e 100644
--- a/demos/hello_wolf/feature_range_bearing.cpp
+++ b/demos/hello_wolf/feature_range_bearing.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * FeatureRangeBearing2d.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
#include "feature_range_bearing.h"
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 004e4e720..be6851b6f 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * processor_range_bearing.cpp
- *
- * Created on: Nov 30, 2017
- * Author: jsola
- */
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
diff --git a/demos/solver/test_SPQR.cpp b/demos/solver/test_SPQR.cpp
index 341204219..66a17bdeb 100644
--- a/demos/solver/test_SPQR.cpp
+++ b/demos/solver/test_SPQR.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * test_SPQR.cpp
- *
- * Created on: Jun 18, 2015
- * Author: jvallve
- */
#include <iostream>
#include <Eigen/SPQRSupport>
diff --git a/demos/solver/test_ccolamd.cpp b/demos/solver/test_ccolamd.cpp
index 1f168493d..5a9bef001 100644
--- a/demos/solver/test_ccolamd.cpp
+++ b/demos/solver/test_ccolamd.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * test_ccolamd.cpp
- *
- * Created on: Jun 11, 2015
- * Author: jvallve
- */
// Wolf includes
#include "core/common/wolf.h"
diff --git a/demos/solver/test_ccolamd_blocks.cpp b/demos/solver/test_ccolamd_blocks.cpp
index 11e1148ea..3351beb34 100644
--- a/demos/solver/test_ccolamd_blocks.cpp
+++ b/demos/solver/test_ccolamd_blocks.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
//std includes
#include <cstdlib>
diff --git a/demos/solver/test_iQR.cpp b/demos/solver/test_iQR.cpp
index e880992a3..4ab060512 100644
--- a/demos/solver/test_iQR.cpp
+++ b/demos/solver/test_iQR.cpp
@@ -17,19 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * test_iQR.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
-
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
//std includes
#include <cstdlib>
diff --git a/demos/solver/test_iQR_wolf.cpp b/demos/solver/test_iQR_wolf.cpp
index 06fdf707e..041057ba1 100644
--- a/demos/solver/test_iQR_wolf.cpp
+++ b/demos/solver/test_iQR_wolf.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * test_iQR_wolf.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
//std includes
#include <cstdlib>
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index cb47a37cd..2371a3dd9 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * test_iQR_wolf.cpp
- *
- * Created on: Jun 17, 2015
- * Author: jvallve
- */
//std includes
#include <cstdlib>
diff --git a/demos/solver/test_incremental_ccolamd_blocks.cpp b/demos/solver/test_incremental_ccolamd_blocks.cpp
index c7d83a011..07f9b7f6a 100644
--- a/demos/solver/test_incremental_ccolamd_blocks.cpp
+++ b/demos/solver/test_incremental_ccolamd_blocks.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * test_ccolamd_blocks.cpp
- *
- * Created on: Jun 12, 2015
- * Author: jvallve
- */
//std includes
#include <cstdlib>
diff --git a/demos/solver/test_permutations.cpp b/demos/solver/test_permutations.cpp
index 0e43cd1f0..94dcad3d9 100644
--- a/demos/solver/test_permutations.cpp
+++ b/demos/solver/test_permutations.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * test_permutations.cpp
- *
- * Created on: Jun 15, 2015
- * Author: jvallve
- */
//std includes
#include <cstdlib>
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 3f19e1f1f..ad0a33720 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -149,8 +149,10 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Factory method to install (create, and add to sensor) processors only from a YAML node -- Helper method
* loading parameters from file
* \param _processor_node YAML node containing all necessary information to call the factory and create the processor.
+ * \param _folders_schema a vector of paths where the schema files (to validate the YAML node) are placed, if empty the node is not validated.
*/
- ProcessorBasePtr installProcessor(const YAML::Node& _processor_node);
+ ProcessorBasePtr installProcessor(const YAML::Node& _processor_node,
+ const std::vector<std::string>& _folders_schema = {});
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index d1563a43f..3422c8361 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -99,7 +99,7 @@ namespace wolf
* To create a ProcessorOdom2d, you type:
*
* \code
- * auto camera_ptr = FactoryProcessor::create("ProcessorOdom2d", yaml_node);
+ * auto camera_ptr = FactoryProcessorNode::create("ProcessorOdom2d", yaml_node);
* \endcode
*
* or:
@@ -113,9 +113,10 @@ namespace wolf
*
*/
typedef Factory<ProcessorBasePtr,
- const YAML::Node&> FactoryProcessor;
+ const YAML::Node&,
+ const std::vector<std::string>&> FactoryProcessorNode;
template<>
-inline std::string FactoryProcessor::getClass() const
+inline std::string FactoryProcessorNode::getClass() const
{
return "FactoryProcessor";
}
@@ -130,8 +131,8 @@ inline std::string FactoryProcessorYaml::getClass() const
}
#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
- namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \
- wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
+ namespace{ const bool WOLF_UNUSED ProcessorType##NodeRegistered = \
+ wolf::FactoryProcessorNode::registerCreator(#ProcessorType, ProcessorType::create); } \
namespace{ const bool WOLF_UNUSED ProcessorType##YamlRegistered = \
wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); } \
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index be6eeb8be..71c064d00 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -23,33 +23,10 @@
#include "core/common/wolf.h"
#include "core/composite/vector_composite.h"
#include "core/composite/type_composite.h"
-#include "core/common/params_base.h" // for toString
#include "yaml-cpp/yaml.h"
namespace wolf
{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider);
-
-struct ParamsMotionProvider
-{
- bool state_provider = true;
- int state_provider_order = 1;
-
- ParamsMotionProvider() = default;
- ParamsMotionProvider(const YAML::Node& _n)
- {
- state_provider = _n["state_provider"].as<bool>();
- if (state_provider)
- state_provider_order = _n["state_provider_order"].as<double>();
- }
- std::string print() const
- {
- return "state_provider: " + toString(state_provider) + "\n"
- + "state_provider_order: " + toString(state_provider_order) + "\n";
- }
-
-};
class TimeStamp;
WOLF_PTR_TYPEDEFS(MotionProvider);
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index a9795cd3b..6c0662131 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -456,7 +456,7 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr
const YAML::Node& _params,
const std::vector<std::string> _folders_schema)
{
- std::shared_ptr<classType> prc = std::make_shared<classType>(_params, _folders_schema);
+ std::shared_ptr<classType> prc = std::static_pointer_cast<classType>(classType::create(_params, _folders_schema));
prc->configure(_sen_ptr);
prc->link(_sen_ptr);
return prc;
diff --git a/src/capture/capture_odom_2d.cpp b/src/capture/capture_odom_2d.cpp
index 447a301ef..c9d3b6550 100644
--- a/src/capture/capture_odom_2d.cpp
+++ b/src/capture/capture_odom_2d.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * capture_odom_2d.cpp
- *
- * Created on: Oct 16, 2017
- * Author: jsola
- */
#include "core/capture/capture_odom_2d.h"
diff --git a/src/capture/capture_odom_3d.cpp b/src/capture/capture_odom_3d.cpp
index eb3b3bff5..f359b7e96 100644
--- a/src/capture/capture_odom_3d.cpp
+++ b/src/capture/capture_odom_3d.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * capture_odom_3d.cpp
- *
- * Created on: Oct 16, 2017
- * Author: jsola
- */
#include "core/capture/capture_odom_3d.h"
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 0e3e8c701..984af4d6e 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * qr_manager.cpp
- *
- * Created on: Jun 7, 2017
- * Author: jvallve
- */
#include "qr_manager.h"
diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp
index f5a3ab76e..c80cc7c93 100644
--- a/src/feature/feature_motion.cpp
+++ b/src/feature/feature_motion.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * feature_motion.cpp
- *
- * Created on: Aug 11, 2017
- * Author: jsola
- */
#include "core/feature/feature_motion.h"
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index ed83df692..b1a608acb 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -119,14 +119,14 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file,
const std::vector<std::string>& _primary_schema_folders)
{
- // schema folders
+ // schema folders
// (optional _primary_schema_folders specify folders where to search schemas before installed ones)
std::vector<std::string> schema_folders = _primary_schema_folders;
schema_folders.push_back(_WOLF_SCHEMA_DIR);
// search plugins to load them temporarily and get the schema folders
std::set<YAML::Node> visited_nodes;
- auto plugins = searchPlugins(YAML::Load(_input_yaml_file), visited_nodes);
+ auto plugins = searchPlugins(YAML::Load(_input_yaml_file), visited_nodes);
for (auto plugin : plugins)
{
// temporary load to automatically register schema folders
@@ -214,7 +214,7 @@ ProblemPtr Problem::autoSetup(YAML::Node _param_node)
for (auto processor_n : processors_node)
{
auto processor_type = processor_n["type"].as<std::string>();
- if (not FactoryProcessor::isCreatorRegistered(processor_type))
+ if (not FactoryProcessorNode::isCreatorRegistered(processor_type))
{
problem->loadPlugin(processor_n["plugin"].as<std::string>());
}
@@ -317,10 +317,10 @@ Problem::~Problem()
// WOLF_DEBUG("destructed -P");
}
-SensorBasePtr Problem::installSensor(const YAML::Node& _sensor_node,
- const std::vector<std::string>& _folders_schema)
+SensorBasePtr Problem::installSensor(const YAML::Node& _sensor_node, const std::vector<std::string>& _folders_schema)
{
- SensorBasePtr sen_ptr = FactorySensorNode::create(_sensor_node["type"].as<std::string>(), _sensor_node, _folders_schema);
+ SensorBasePtr sen_ptr =
+ FactorySensorNode::create(_sensor_node["type"].as<std::string>(), _sensor_node, _folders_schema);
sen_ptr->link(getHardware());
return sen_ptr;
}
@@ -339,7 +339,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type,
return sen_ptr;
}
-ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node)
+ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node,
+ const std::vector<std::string>& _folders_schema)
{
auto corresponding_sensor_name = _processor_node["sensor_name"].as<std::string>();
SensorBasePtr sen_ptr = findSensor(corresponding_sensor_name);
@@ -348,7 +349,7 @@ ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node)
"\" not found. Check sensor name, it must match in sensor and processor!");
auto processor_type = _processor_node["type"].as<std::string>();
- ProcessorBasePtr prc_ptr = FactoryProcessor::create(processor_type, _processor_node);
+ ProcessorBasePtr prc_ptr = FactoryProcessorNode::create(processor_type, _processor_node, _folders_schema);
// Dimension check
int prc_dim = prc_ptr->getDim();
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index fff15d4b2..46d93f01e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * ProcessorTracker.cpp
- *
- * Created on: Apr 7, 2016
- * Author: jvallve
- */
// wolf
#include "core/processor/processor_tracker.h"
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index bc8d52ede..bd5dc55b4 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * \processor_tracker_feature.cpp
- *
- * Created on: 27/02/2016
- * \author: jsola
- */
#include "core/processor/processor_tracker_feature.h"
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index b76235c1f..e44361a99 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -57,37 +57,38 @@ wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
target_link_libraries(gtest_emplace PUBLIC dummy)
-# # FeatureBase classes test
-# wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
+# FeatureBase classes test
+wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
-# # FrameBase classes test
-# wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
+# FrameBase classes test
+wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
-# # GraphSearch class test
-# wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
+# GraphSearch class test
+wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
-# # MotionProvider classes test
-# wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
+# MotionProvider classes test
+wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
+target_link_libraries(gtest_motion_provider PUBLIC dummy)
-# # LocalParametrizationXxx classes test
-# wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
+# LocalParametrizationXxx classes test
+wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
-# # Logging test
-# wolf_add_gtest(gtest_logging gtest_logging.cpp)
+# Logging test
+wolf_add_gtest(gtest_logging gtest_logging.cpp)
-# # MotionBuffer class test
-# wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
+# MotionBuffer class test
+wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
-# # NodeStateBlocks classes test
-# wolf_add_gtest(gtest_node_state_blocks gtest_node_state_blocks.cpp)
+# NodeStateBlocks classes test
+wolf_add_gtest(gtest_node_state_blocks gtest_node_state_blocks.cpp)
# # Problem class test
# wolf_add_gtest(gtest_problem gtest_problem.cpp)
# target_link_libraries(gtest_problem PUBLIC dummy)
-# # ProcessorBase class test
-# wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
-# target_link_libraries(gtest_processor_base PUBLIC dummy)
+# ProcessorBase class test
+wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
+target_link_libraries(gtest_processor_base PUBLIC dummy)
# # ProcessorMotion class test
# wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
@@ -228,8 +229,8 @@ wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
# # Schema test
# wolf_add_gtest(gtest_schema gtest_schema.cpp)
-# # SensorDiffDrive class test
-# wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
+# SensorDiffDrive class test
+wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
# # SensorOdom class test
# wolf_add_gtest(gtest_sensor_odom gtest_sensor_odom.cpp)
diff --git a/test/dummy/ProcessorMotionProviderDummy.schema b/test/dummy/ProcessorMotionProviderDummy.schema
new file mode 100644
index 000000000..8888e2428
--- /dev/null
+++ b/test/dummy/ProcessorMotionProviderDummy.schema
@@ -0,0 +1,2 @@
+follow: ProcessorBase.schema
+follow: MotionProvider.schema
\ No newline at end of file
diff --git a/test/dummy/ProcessorMotionProviderDummyPOV.schema b/test/dummy/ProcessorMotionProviderDummyPOV.schema
new file mode 100644
index 000000000..8888e2428
--- /dev/null
+++ b/test/dummy/ProcessorMotionProviderDummyPOV.schema
@@ -0,0 +1,2 @@
+follow: ProcessorBase.schema
+follow: MotionProvider.schema
\ No newline at end of file
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
index 648cd48bc..c619e86eb 100644
--- a/test/dummy/processor_motion_provider_dummy.h
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -28,17 +28,17 @@ namespace wolf
WOLF_PTR_TYPEDEFS(MotionProviderDummy);
-class MotionProviderDummy : public ProcessorBase, public MotionProvider
+class ProcessorMotionProviderDummy : public ProcessorBase, public MotionProvider
{
public:
- MotionProviderDummy(const YAML::Node& _params) :
- ProcessorBase("MotionProviderDummy", 2, _params),
+ ProcessorMotionProviderDummy(const YAML::Node& _params) :
+ ProcessorBase("ProcessorMotionProviderDummy", 2, _params),
MotionProvider({{'P',"StatePoint2d"},{'O',"StateAngle"}}, _params)
{}
- ~MotionProviderDummy(){};
+ ~ProcessorMotionProviderDummy(){};
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(MotionProviderDummy);
+ WOLF_PROCESSOR_CREATE(ProcessorMotionProviderDummy);
void configure(SensorBasePtr _sensor) override {};
void processCapture(CaptureBasePtr) override {};
@@ -66,5 +66,5 @@ class MotionProviderDummy : public ProcessorBase, public MotionProvider
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR(MotionProviderDummy);
+WOLF_REGISTER_PROCESSOR(ProcessorMotionProviderDummy);
} // namespace wolf
diff --git a/test/dummy/processor_motion_provider_dummy_pov.h b/test/dummy/processor_motion_provider_dummy_pov.h
new file mode 100644
index 000000000..a54ecacaa
--- /dev/null
+++ b/test/dummy/processor_motion_provider_dummy_pov.h
@@ -0,0 +1,70 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+#include <core/processor/motion_provider.h>
+#include "core/processor/processor_base.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(MotionProviderDummy);
+
+class ProcessorMotionProviderDummyPOV : public ProcessorBase, public MotionProvider
+{
+ public:
+ ProcessorMotionProviderDummyPOV(const YAML::Node& _params) :
+ ProcessorBase("ProcessorMotionProviderDummyPOV", 2, _params),
+ MotionProvider({{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}}, _params)
+ {}
+ ~ProcessorMotionProviderDummyPOV(){};
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorMotionProviderDummyPOV);
+
+ void configure(SensorBasePtr _sensor) override {};
+ void processCapture(CaptureBasePtr) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
+ bool triggerInCapture(CaptureBasePtr) const override { return false; };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override { return false; };
+ bool storeKeyFrame(FrameBasePtr) override { return false; };
+ bool storeCapture(CaptureBasePtr) override { return false; };
+ bool voteForKeyFrame() const override { return false; };
+ TimeStamp getTimeStamp() const override {return TimeStamp(0);};
+
+ VectorComposite getState(const StateKeys& _structure = "") const override
+ {
+ return getOdometry();
+ };
+
+ VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override
+ {
+ return getOdometry();
+ };
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorMotionProviderDummyPOV);
+} // namespace wolf
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index f36b11d10..c0d16d1b6 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -83,24 +83,24 @@ TEST(Emplace, Processor)
ProblemPtr P = Problem::create("PO", 2);
auto S = SensorBase::emplace<SensorDummy2d>(
- P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml"));
+ P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml"), {wolf_dir});
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
ASSERT_EQ(P->getHardware()->getSensorList().front(), S);
auto prc =
- ProcessorOdom2d::emplace<ProcessorOdom2d>(S, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2D.yaml"));
+ ProcessorBase::emplace<ProcessorOdom2d>(S, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
ASSERT_EQ(P, prc->getProblem());
ASSERT_EQ(S, prc->getSensor());
ASSERT_EQ(prc, S->getProcessorList().front());
SensorBasePtr sen2 = SensorBase::emplace<SensorDummy2d>(
- P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml"));
+ P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml"), {wolf_dir});
ProcessorOdom2dPtr prc2 =
- ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2D.yaml"));
+ ProcessorBase::emplace<ProcessorOdom2d>(sen2, YAML::LoadFile(wolf_dir + "/test/yaml/processor_odom_2d.yaml"), {wolf_dir});
ASSERT_EQ(P, prc->getProblem());
ASSERT_EQ(sen2, prc2->getSensor());
@@ -203,7 +203,7 @@ TEST(Emplace, EmplaceDerived)
ProblemPtr P = Problem::create("PO", 2);
auto S = SensorBase::emplace<SensorOdom2d>(
- P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_odom_2d.yaml"));
+ P->getHardware(), YAML::LoadFile(wolf_dir + "/test/yaml/sensor_odom_2d.yaml"), {wolf_dir});
ASSERT_EQ(P, S->getProblem());
ASSERT_EQ(P->getHardware(), S->getHardware());
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index bf28bfcfa..83f04481e 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_factory_state_block.cpp
- *
- * Created on: Apr 28, 2020
- * Author: jsola
- */
#include "core/utils/utils_gtest.h"
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 716653011..2cf2ec040 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_feature_base.cpp
- *
- * Created on: Apr 10, 2017
- * Author: jsola
- */
#include "core/feature/feature_base.h"
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index a74d7fd72..69aec4336 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -87,11 +87,7 @@ TEST(FrameBase, LinksToTree)
auto F2 =
FrameBase::emplace<FrameBase>(T, 2, make_shared<StatePoint2d>(Vector2d::Zero()), make_shared<StateAngle>(0));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), nullptr);
- WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
- auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
- WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
- p->link(S);
- WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+ auto p = P->installProcessor("ProcessorOdom2d", S, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir});
auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double, 1, 1>::Identity() * .01);
auto fac = FactorBase::emplace<FactorRelativePose2d>(
f, Vector3d::Random(), Matrix3d::Identity(), F1, F2, p, false, TOP_MOTION);
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 3f2e06e16..7a9f35d46 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -17,16 +17,9 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_motion_buffer.cpp
- *
- * Created on: Nov 12, 2016
- * Author: jsola
- */
#include "core/utils/utils_gtest.h"
-
#include "core/processor/motion_buffer.h"
#include "core/common/wolf.h"
diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp
index 5ae1c7635..a70a70881 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -18,8 +18,9 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//Wolf
+// Wolf
#include "dummy/processor_motion_provider_dummy.h"
+#include "dummy/processor_motion_provider_dummy_pov.h"
#include "core/processor/motion_provider.h"
#include "core/utils/utils_gtest.h"
@@ -35,58 +36,66 @@
using namespace wolf;
using namespace Eigen;
-
class MotionProviderTest : public testing::Test
{
- public:
- ProblemPtr problem;
- SensorBasePtr sen;
- ProcessorBasePtr prc1, prc2, prc3;
- MotionProviderPtr im1, im2, im3;
-
- std::string wolf_dir = _WOLF_CODE_DIR;
- double dt = 0.01;
-
- void SetUp() override
- {
- // Wolf problem
- problem = Problem::create("POV", 2);
-
- // Install odom sensor
- sen = problem->installSensor("SensorOdom2d",
- wolf_dir + "/test/yaml/sensor_odom_2d.yaml",
+ public:
+ ProblemPtr problem;
+ SensorBasePtr sen;
+ ProcessorBasePtr prc1, prc2, prc3;
+ MotionProviderPtr im1, im2, im3;
+
+ std::string wolf_dir = _WOLF_CODE_DIR;
+
+ void SetUp() override
+ {
+ // Wolf problem
+ problem = Problem::create("POV", 2);
+
+ // Install odom sensor
+ sen = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+
+ // Install 3 odom processors
+ // ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>();
+ // prc1_params->time_tolerance = dt/2;
+ // prc1_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
+ // prc1_params->state_provider = false;
+ // prc1_params->name = "not getter processor";
+
+ // prc1 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc1_params);
+ prc1 = problem->installProcessor("ProcessorMotionProviderDummy",
+ sen,
+ wolf_dir + "/test/yaml/processor_motion_provider_dummy1.yaml",
{wolf_dir});
-
- // Install 3 odom processors
- ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>();
- prc1_params->time_tolerance = dt/2;
- prc1_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
- prc1_params->state_provider = false;
- prc1_params->name = "not getter processor";
-
- prc1 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc1_params);
- im1 = std::dynamic_pointer_cast<MotionProvider>(prc1);
-
- ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>();
- prc2_params->time_tolerance = dt/2;
- prc2_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
- prc2_params->state_provider = true;
- prc2_params->state_provider_order = 1;
- prc1_params->name = "getter processor";
-
- prc2 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc2_params);
- im2 = std::dynamic_pointer_cast<MotionProvider>(prc2);
-
- ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>();
- prc3_params->time_tolerance = dt/2;
- prc3_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}};
- prc3_params->state_provider = true;
- prc3_params->state_provider_order = 1;
- prc1_params->name = "getter processor lower priority";
-
- prc3 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc3_params);
- im3 = std::dynamic_pointer_cast<MotionProvider>(prc3);
- }
+ im1 = std::dynamic_pointer_cast<MotionProvider>(prc1);
+
+ // ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>();
+ // prc2_params->time_tolerance = dt / 2;
+ // prc2_params->state_types = {{'P', "StatePoint2d"}, {'O', "StateAngle"}};
+ // prc2_params->state_provider = true;
+ // prc2_params->state_provider_order = 1;
+ // prc1_params->name = "getter processor";
+
+ // prc2 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc2_params);
+ prc2 = problem->installProcessor("ProcessorMotionProviderDummy",
+ sen,
+ wolf_dir + "/test/yaml/processor_motion_provider_dummy2.yaml",
+ {wolf_dir});
+ im2 = std::dynamic_pointer_cast<MotionProvider>(prc2);
+
+ // ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>();
+ // prc3_params->time_tolerance = dt / 2;
+ // prc3_params->state_types = {{'P', "StatePoint2d"}, {'O', "StateAngle"}, {'V', "StateVector2d"}};
+ // prc3_params->state_provider = true;
+ // prc3_params->state_provider_order = 1;
+ // prc1_params->name = "getter processor lower priority";
+
+ // prc3 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc3_params);
+ prc3 = problem->installProcessor("ProcessorMotionProviderDummyPOV",
+ sen,
+ wolf_dir + "/test/yaml/processor_motion_provider_dummy3.yaml",
+ {wolf_dir});
+ im3 = std::dynamic_pointer_cast<MotionProvider>(prc3);
+ }
};
/*
@@ -102,19 +111,19 @@ class MotionProviderTest : public testing::Test
TEST_F(MotionProviderTest, install)
{
// All MotionProvider() = true
- ASSERT_TRUE (prc1->isMotionProvider());
- ASSERT_TRUE (prc2->isMotionProvider());
- ASSERT_TRUE (prc3->isMotionProvider());
- ASSERT_TRUE (im1 != nullptr);
- ASSERT_TRUE (im2 != nullptr);
- ASSERT_TRUE (im3 != nullptr);
+ ASSERT_TRUE(prc1->isMotionProvider());
+ ASSERT_TRUE(prc2->isMotionProvider());
+ ASSERT_TRUE(prc3->isMotionProvider());
+ ASSERT_TRUE(im1 != nullptr);
+ ASSERT_TRUE(im2 != nullptr);
+ ASSERT_TRUE(im3 != nullptr);
// well configured
ASSERT_FALSE(im1->isStateGetter());
ASSERT_TRUE(im2->isStateGetter());
ASSERT_TRUE(im3->isStateGetter());
ASSERT_EQ(im2->getOrder(), 1);
- ASSERT_EQ(im3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
+ ASSERT_EQ(im3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
ASSERT_TRUE(im1->getStateTypes().has('P'));
ASSERT_TRUE(im1->getStateTypes().has('O'));
ASSERT_FALSE(im1->getStateTypes().has('V'));
@@ -133,17 +142,17 @@ TEST_F(MotionProviderTest, install)
TEST_F(MotionProviderTest, odometry)
{
- VectorComposite odom_p("P",{Vector2d::Zero()});
- VectorComposite odom_pov("POV",{Vector2d::Zero(),Vector1d::Zero(),Vector2d::Zero()});
+ VectorComposite odom_p("P", {Vector2d::Zero()});
+ VectorComposite odom_pov("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
// Error: required PO keys to be added
- ASSERT_DEATH({im1->setOdometry(odom_p);},"");
+ ASSERT_DEATH({ im1->setOdometry(odom_p); }, "");
im1->setOdometry(odom_pov);
im2->setOdometry(odom_pov);
im3->setOdometry(odom_pov);
// im1 ->set odom = 0, 0, 0
- VectorComposite odom1("PO",{Vector2d::Zero(),Vector1d::Zero()});
+ VectorComposite odom1("PO", {Vector2d::Zero(), Vector1d::Zero()});
im1->setOdometry(odom1);
auto odom1_get = im1->getOdometry();
EXPECT_TRUE(odom1_get.count('P') == 1);
@@ -152,7 +161,7 @@ TEST_F(MotionProviderTest, odometry)
EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9);
// im1 ->set odom = 1, 1, 1
- VectorComposite odom2("PO",{Vector2d::Ones(),Vector1d::Ones()});
+ VectorComposite odom2("PO", {Vector2d::Ones(), Vector1d::Ones()});
im2->setOdometry(odom2);
auto odom2_get = im2->getOdometry();
EXPECT_TRUE(odom2_get.count('P') == 1);
@@ -161,7 +170,7 @@ TEST_F(MotionProviderTest, odometry)
EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9);
// im1 ->set odom = 2, 2, 2, 2, 2
- VectorComposite odom3("POV",{2 * Vector2d::Ones(), 2 * Vector1d::Ones(), 2 * Vector2d::Ones()});
+ VectorComposite odom3("POV", {2 * Vector2d::Ones(), 2 * Vector1d::Ones(), 2 * Vector2d::Ones()});
im3->setOdometry(odom3);
auto odom3_get = im3->getOdometry();
EXPECT_TRUE(odom3_get.count('P') == 1);
@@ -192,6 +201,6 @@ TEST_F(MotionProviderTest, odometry)
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 9c9e9571f..96c3ba38a 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -17,14 +17,8 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_capture_base.cpp
- *
- * Created on: Feb 15, 2018
- * Author: asantamaria
- */
-
-//Wolf
+
+// Wolf
#include "core/utils/utils_gtest.h"
#include "core/processor/processor_odom_2d.h"
@@ -45,97 +39,75 @@
using namespace wolf;
using namespace Eigen;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
TEST(ProcessorBase, MotionProvider)
{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using Eigen::Vector2d;
-
- std::string wolf_dir = _WOLF_CODE_DIR;
-
- double dt = 0.01;
-
// Wolf problem
ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor)
- auto sens_trk = problem->installSensor("SensorDummy2d",
- wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml",
- {wolf_dir});
- auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
- proc_trk_params->name = "proc tracker";
- auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
+ auto sens_trk = problem->installSensor(
+ "SensorDummy2d", wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_dir});
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy",
+ sens_trk,
+ wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml",
+ {wolf_dir});
// Install odometer (sensor and processor)
- auto sens_odo = problem->installSensor("SensorOdom2d",
- wolf_dir + "/test/yaml/sensor_odom_2d.yaml",
- {wolf_dir});
- auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
- proc_odo_params->time_tolerance = dt/2;
- proc_odo_params->name = "odom processor";
- auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params);
+ auto sens_odo = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto proc_odo = problem->installProcessor(
+ "ProcessorOdom2d", sens_trk, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir});
ASSERT_FALSE(proc_trk->isMotionProvider());
- ASSERT_TRUE (proc_odo->isMotionProvider());
+ ASSERT_TRUE(proc_odo->isMotionProvider());
}
-
TEST(ProcessorBase, KeyFrameCallback)
{
-
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using Eigen::Vector2d;
-
- std::string wolf_dir = _WOLF_CODE_DIR;
-
- double dt = 0.01;
+ double dt = 1;
// Wolf problem
ProblemPtr problem = Problem::create("PO", 2);
// Install tracker (sensor and processor)
- auto sens_trk = problem->installSensor("SensorDummy2d",
- wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml",
- {wolf_dir});
- auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
- proc_trk_params->name = "proc tracker";
- auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
+ auto sens_trk = problem->installSensor(
+ "SensorDummy2d", wolf_dir + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_dir});
+ auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy",
+ sens_trk,
+ wolf_dir + "/test/yaml/processor_tracker_feature_dummy.yaml",
+ {wolf_dir});
+ proc_trk->setVotingActive(false);
// Install odometer (sensor and processor)
- auto sens_odo = problem->installSensor("SensorOdom2d",
- wolf_dir + "/test/yaml/sensor_odom_2d.yaml",
- {wolf_dir});
- auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
- proc_odo_params->time_tolerance = dt/2;
- proc_odo_params->name = "odom processor";
- auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params);
+ auto sens_odo = problem->installSensor("SensorOdom2d", wolf_dir + "/test/yaml/sensor_odom_2d.yaml", {wolf_dir});
+ auto proc_odo = problem->installProcessor(
+ "ProcessorOdom2d", sens_trk, wolf_dir + "/test/yaml/processor_odom_2d.yaml", {wolf_dir});
+ proc_odo->setTimeTolerance(dt / 2);
+ proc_odo->setVotingActive(false);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
// Sequence to test Key Frame creations (callback calls)
// initialize
- TimeStamp t(0.0);
- SpecStateComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))},
- {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(sqrt(0.1)))}};
+ TimeStamp t(0.0);
+ SpecStateComposite prior{
+ {'P', SpecState("StatePoint2d", Vector2d(0, 0), "factor", Vector2d(sqrt(0.1), sqrt(0.1)))},
+ {'O', SpecState("StateAngle", Vector1d(0), "factor", Vector1d(sqrt(0.1)))}};
problem->setPrior(prior, t);
- CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
+ CaptureOdom2dPtr capt_odo = std::make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5, 0));
// Track
- CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk));
+ CaptureVoidPtr capt_trk(std::make_shared<CaptureVoid>(t, sens_trk));
proc_trk->captureCallback(capt_trk);
- for (size_t ii=0; ii<10; ii++ )
+ for (size_t ii = 0; ii < 10; ii++)
{
// Move
- t = t+dt;
- WOLF_INFO("----------------------- ts: ", t , " --------------------------");
+ t = t + dt;
+ WOLF_INFO("----------------------- ts: ", t, " --------------------------");
std::cout << "1\n";
capt_odo->setTimeStamp(t);
@@ -144,24 +116,29 @@ TEST(ProcessorBase, KeyFrameCallback)
std::cout << "3\n";
// Track
- capt_trk = make_shared<CaptureVoid>(t, sens_trk);
+ capt_trk = std::make_shared<CaptureVoid>(t, sens_trk);
std::cout << "4\n";
- problem->print(4,1,1,1, std::cout);
+ problem->print(4, 1, 1, 1, std::cout);
proc_trk->captureCallback(capt_trk);
std::cout << "5\n";
- problem->print(4,1,1,0);
+ // keyframe creation
+ if (ii == 5)
+ problem->emplaceFrame(t, SpecStateComposite{{'P', SpecState("StatePoint2d", Vector2d(0, 0), "initial_guess")},
+ {'O', SpecState("StateAngle", Vector1d(0), "initial_guess")}});
+
+ problem->print(4, 1, 1, 0);
std::cout << "6\n";
// Only odom creating KFs
- ASSERT_EQ( problem->getLastFrame()->getKeys().size(),2);
- ASSERT_TRUE( problem->getLastFrame()->getKeys().find('P') != std::string::npos);
- ASSERT_TRUE( problem->getLastFrame()->getKeys().find('O') != std::string::npos);
+ ASSERT_EQ(problem->getLastFrame()->getKeys().size(), 2);
+ ASSERT_TRUE(problem->getLastFrame()->getKeys().find('P') != std::string::npos);
+ ASSERT_TRUE(problem->getLastFrame()->getKeys().find('O') != std::string::npos);
}
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_schema.cpp b/test/gtest_schema.cpp
index a969a201b..95c428aa8 100644
--- a/test/gtest_schema.cpp
+++ b/test/gtest_schema.cpp
@@ -90,7 +90,7 @@ TEST(Schema, check_schema_existence)
}
// FactoryProcessor
- auto registered_processors = FactoryProcessor::getRegisteredKeys();
+ auto registered_processors = FactoryProcessorNode::getRegisteredKeys();
for (auto key : registered_processors)
{
EXPECT_TRUE(existsSchema(key));
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index 55259840c..6b7c4bf1b 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -33,15 +33,28 @@ Vector2d p_state = (Vector2d() << 1, 2).finished();
Vector1d o_state = (Vector1d() << 3).finished();
Vector3d i_state = (Vector3d() << 0.1, 0.2, 0.3).finished();
-TEST(SensorDiffDrive, constructor_priors)
+TEST(SensorDiffDrive, create)
{
- auto param = std::make_shared<ParamsSensorDiffDrive>();
-
- SpecStateSensorComposite priors{{'P', SpecStateSensor("StatePoint2d", p_state, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", o_state, "fix")}, // default not dynamic
- {'I', SpecStateSensor("StateParams3", i_state, "fix")}}; // default not dynamic
-
- auto sen = SensorDiffDrive::create(param, priors);
+ YAML::Node param;
+
+ param["states"]["P"]["type"] = "StatePoint2d";
+ param["states"]["P"]["state"] = p_state;
+ param["states"]["P"]["mode"] = "fix";
+ param["states"]["P"]["dynamic"] = false;
+ param["states"]["O"]["type"] = "StateAngle";
+ param["states"]["O"]["state"] = o_state;
+ param["states"]["O"]["mode"] = "fix";
+ param["states"]["O"]["dynamic"] = false;
+ param["states"]["I"]["type"] = "StateParams3";
+ param["states"]["I"]["state"] = i_state;
+ param["states"]["I"]["mode"] = "fix";
+ param["states"]["I"]["dynamic"] = false;
+
+ param["name"] = "just a sensor";
+ param["ticks_per_wheel_revolution"] = 4;
+ param["ticks_std_factor"] = 2;
+
+ auto sen = SensorDiffDrive::create(param, {wolf_dir});
ASSERT_NE(sen, nullptr);
@@ -52,7 +65,7 @@ TEST(SensorDiffDrive, constructor_priors)
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
}
-TEST(SensorDiffDrive, factory)
+TEST(SensorDiffDrive, factory_node)
{
yaml_schema_cpp::YamlServer server({wolf_dir}, wolf_dir + "/test/yaml/sensor_diff_drive.yaml");
@@ -60,7 +73,7 @@ TEST(SensorDiffDrive, factory)
WOLF_WARN_COND(not valid, server.getLog());
ASSERT_TRUE(valid);
- auto sb = FactorySensorNode::create("SensorDiffDrive", server.getNode());
+ auto sb = FactorySensorNode::create("SensorDiffDrive", server.getNode(), {wolf_dir});
SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
@@ -75,10 +88,9 @@ TEST(SensorDiffDrive, factory)
TEST(SensorDiffDrive, factory_yaml)
{
- auto sb = FactorySensorFile::create(
- "SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive.yaml", {wolf_dir});
+ auto sb = FactorySensorFile::create("SensorDiffDrive", wolf_dir + "/test/yaml/sensor_diff_drive.yaml", {wolf_dir});
- SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
+ SensorDiffDrivePtr sen = std::static_pointer_cast<SensorDiffDrive>(sb);
ASSERT_NE(sen, nullptr);
@@ -91,34 +103,26 @@ TEST(SensorDiffDrive, factory_yaml)
TEST(SensorDiffDrive, getParams)
{
- auto param = std::make_shared<ParamsSensorDiffDrive>();
- param->ticks_per_wheel_revolution = 400;
- param->ticks_std_factor = 2;
+ YAML::Node param = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_diff_drive.yaml");
+ param["ticks_per_wheel_revolution"] = 400;
+ param["ticks_std_factor"] = 2;
- SpecStateSensorComposite priors{{'P', SpecStateSensor("StatePoint2d", p_state, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", o_state, "fix")}, // default not dynamic
- {'I', SpecStateSensor("StateParams3", i_state, "fix")}}; // default not dynamic
-
- auto sen = std::static_pointer_cast<SensorDiffDrive>(SensorDiffDrive::create(param, priors));
+ auto sen = std::static_pointer_cast<SensorDiffDrive>(SensorDiffDrive::create(param, {wolf_dir}));
ASSERT_NE(sen->getParams(), nullptr);
- ASSERT_NEAR(sen->getParams()->ticks_per_wheel_revolution, 400, Constants::EPS);
+ ASSERT_NEAR(sen->getTicksPerWheelRevolution(), 400, Constants::EPS);
ASSERT_NEAR(sen->getRadiansPerTick(), 2 * M_PI / 400, Constants::EPS);
- ASSERT_NEAR(sen->getParams()->ticks_std_factor, 2, Constants::EPS);
+ ASSERT_NEAR(sen->getTicksStdFactor(), 2, Constants::EPS);
}
TEST(SensorDiffDrive, computeNoiseCov)
{
- auto param = std::make_shared<ParamsSensorDiffDrive>();
- param->ticks_per_wheel_revolution = 400;
- param->ticks_std_factor = 2;
-
- SpecStateSensorComposite priors{{'P', SpecStateSensor("StatePoint2d", p_state, "fix")}, // default not dynamic
- {'O', SpecStateSensor("StateAngle", o_state, "fix")}, // default not dynamic
- {'I', SpecStateSensor("StateParams3", i_state, "fix")}}; // default not dynamic
+ YAML::Node param = YAML::LoadFile(wolf_dir + "/test/yaml/sensor_diff_drive.yaml");
+ param["ticks_per_wheel_revolution"] = 400;
+ param["ticks_std_factor"] = 2;
- auto sen = SensorDiffDrive::create(param, priors);
+ auto sen = SensorDiffDrive::create(param, {wolf_dir});
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(Eigen::Vector2d::Zero()), Eigen::Matrix2d::Zero(), Constants::EPS);
ASSERT_MATRIX_APPROX(
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 47721f117..60f18993d 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_solver_manager.cpp
- *
- * Created on: Jun, 2018
- * Author: jvallve
- */
#include "core/utils/utils_gtest.h"
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 0d4fe1ae8..0f3933f66 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_solver_manager.cpp
- *
- * Created on: Jun, 2018
- * Author: jvallve
- */
#include "core/utils/utils_gtest.h"
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index ebc0b11fa..a4d30eda3 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_state_block.cpp
- *
- * Created on: Mar 31, 2020
- * Author: jsola
- */
#include "core/utils/utils_gtest.h"
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index d47b7b899..885b2a0fd 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -17,12 +17,6 @@
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
-/*
- * gtest_trajectory.cpp
- *
- * Created on: Nov 13, 2016
- * Author: jsola
- */
#include "core/utils/utils_gtest.h"
diff --git a/test/yaml/processor_motion_provider_dummy1.yaml b/test/yaml/processor_motion_provider_dummy1.yaml
new file mode 100644
index 000000000..3848617f5
--- /dev/null
+++ b/test/yaml/processor_motion_provider_dummy1.yaml
@@ -0,0 +1,12 @@
+# DO NOT MODIFY THIS FILE! USED BY gtest_motion_provider
+name: "not getter processor"
+
+time_tolerance: 0.5 # seconds
+
+keyframe_vote:
+ voting_active: false
+
+apply_loss_function: false
+
+state_provider: false
+#state_provider_order: 1
\ No newline at end of file
diff --git a/test/yaml/processor_motion_provider_dummy2.yaml b/test/yaml/processor_motion_provider_dummy2.yaml
new file mode 100644
index 000000000..3fc5ecb9a
--- /dev/null
+++ b/test/yaml/processor_motion_provider_dummy2.yaml
@@ -0,0 +1,12 @@
+# DO NOT MODIFY THIS FILE! USED BY gtest_motion_provider
+name: "getter processor"
+
+time_tolerance: 0.5 # seconds
+
+keyframe_vote:
+ voting_active: false
+
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 1
\ No newline at end of file
diff --git a/test/yaml/processor_motion_provider_dummy3.yaml b/test/yaml/processor_motion_provider_dummy3.yaml
new file mode 100644
index 000000000..403035082
--- /dev/null
+++ b/test/yaml/processor_motion_provider_dummy3.yaml
@@ -0,0 +1,12 @@
+# DO NOT MODIFY THIS FILE! USED BY gtest_motion_provider
+name: "getter processor lower priority"
+
+time_tolerance: 0.5 # seconds
+
+keyframe_vote:
+ voting_active: false
+
+apply_loss_function: false
+
+state_provider: true
+state_provider_order: 1 # same as proc2, will be relegated after it
\ No newline at end of file
diff --git a/test/yaml/processor_odom_2d.yaml b/test/yaml/processor_odom_2d.yaml
index bc7dc295b..13c453f9d 100644
--- a/test/yaml/processor_odom_2d.yaml
+++ b/test/yaml/processor_odom_2d.yaml
@@ -1,14 +1,14 @@
-name: a cool processor odom 2d
+name: "a cool processor odom 2d"
-time_tolerance: 0.01 # seconds
+time_tolerance: 0.5 # seconds
keyframe_vote:
- voting_active: false
- max_time_span: 0.2 # seconds
- max_buff_length: 10 # motion deltas
- max_dist_traveled: 0.5 # meters
- max_angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
- cov_det: 1 # determinant of covariance threshold
+ voting_active: false
+ max_time_span: 2 # seconds
+ max_buff_length: 10 # motion deltas
+ max_dist_traveled: 0.5 # meters
+ max_angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
+ cov_det: 1 # determinant of covariance threshold
unmeasured_perturbation_std: 0.001
diff --git a/test/yaml/sensor_diff_drive.yaml b/test/yaml/sensor_diff_drive.yaml
index 340066322..c5dfa8631 100644
--- a/test/yaml/sensor_diff_drive.yaml
+++ b/test/yaml/sensor_diff_drive.yaml
@@ -1,3 +1,4 @@
+# DO NOT CHANGE THE FOLLOWING VALUES they are checked in gtest_sensor_diff_drive
name: a cool sensor diff drive
type: SensorDiffDrive
states:
--
GitLab