diff --git a/CMakeLists.txt b/CMakeLists.txt index 02cd9a270a361a62d9d4f800da6ca7b9694071c0..339376b247cb4e3ad58242c89bca35220e1ecc82 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,6 +131,9 @@ SET(HDRS_SENSOR SET(HDRS_TREE_MANAGER include/${PROJECT_NAME}/tree_manager/tree_manager_sliding_window_tdcp.h ) +SET(HDRS_UTILS + include/${PROJECT_NAME}/utils/load_gnss.h + ) # ============ SOURCES ============ SET(SRCS_CAPTURE @@ -148,6 +151,9 @@ SET(SRCS_SENSOR SET(SRCS_TREE_MANAGER src/tree_manager/tree_manager_sliding_window_tdcp.cpp ) +SET(SRCS_UTILS + src/utils/load_gnss.cpp + ) # create the shared library ADD_LIBRARY(${PLUGIN_NAME} @@ -156,6 +162,7 @@ ADD_LIBRARY(${PLUGIN_NAME} ${SRCS_PROCESSOR} ${SRCS_SENSOR} ${SRCS_TREE_MANAGER} + ${SRCS_UTILS} ) # Set compiler options @@ -228,6 +235,8 @@ INSTALL(FILES ${HDRS_SENSOR} DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor) INSTALL(FILES ${HDRS_TREE_MANAGER} DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/tree_manager) +INSTALL(FILES ${HDRS_UTILS} + DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/utils) INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h" DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal) diff --git a/include/gnss/common/gnss.h b/include/gnss/common/gnss.h index e4e71c39a3656b75de24e3ce453ce694b46ada0a..376ab1880794a45e49e8711a0fdc0825f448fbd7 100644 --- a/include/gnss/common/gnss.h +++ b/include/gnss/common/gnss.h @@ -22,6 +22,7 @@ // Enable project-specific definitions and macros #include "gnss/internal/config.h" +#include "gnss/utils/load_gnss.h" #include <core/common/wolf.h> namespace wolf diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h index 738abbf5f735fe0fc1a69d98417e82768ad0710b..e5e380c79f21272d6e640d847cd6e3e0537d9c22 100644 --- a/include/gnss/sensor/sensor_gnss.h +++ b/include/gnss/sensor/sensor_gnss.h @@ -96,6 +96,8 @@ class SensorGnss : public SensorBase Eigen::Vector3d computeFrameAntennaPosEcef(const FrameBasePtr &) const; Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd &_data) const override; + + static bool gnss_loaded; // used to load the .so easily by the tests }; } // namespace wolf diff --git a/include/gnss/utils/load_gnss.h b/include/gnss/utils/load_gnss.h new file mode 100644 index 0000000000000000000000000000000000000000..66fbd584ed2c0ebd8db14acb5fd8e0ff3db2b82a --- /dev/null +++ b/include/gnss/utils/load_gnss.h @@ -0,0 +1,51 @@ +// 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 + +namespace wolf +{ +// This class is just to force the .so to be easily loaded in tests +class LoadGnss +{ + public: + static bool aux_var; +}; + +#ifdef __GNUC__ +#define WOLF_UNUSED __attribute__((used)) +#elif defined _MSC_VER +#pragma warning(disable : Cxxxxx) +#define WOLF_UNUSED +#elif defined(__LCLINT__) +#define WOLF_UNUSED /*@unused@*/ +#elif defined(__cplusplus) +#define WOLF_UNUSED +#else +#define UNUSED(x) x +#endif + +#define WOLF_LOAD_GNSS \ + namespace \ + { \ + const bool WOLF_UNUSED aux_var_gnss = wolf::LoadGnss::aux_var; \ + } + +} // namespace wolf diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp index 998b35293dbb757eb9e404015920b8f4a6450128..2ac9f892104a410a8e335f0dc334dad122a8e0ea 100644 --- a/src/sensor/sensor_gnss.cpp +++ b/src/sensor/sensor_gnss.cpp @@ -24,9 +24,10 @@ namespace wolf { +bool SensorGnss::gnss_loaded = true; SensorGnss::SensorGnss(const YAML::Node &_params) - : SensorBase("SensorGnss", -1, _params), + : SensorBase("SensorGnss", 0, _params), ENU_defined_(false), t_ENU_MAP_initialized_(false), R_ENU_MAP_initialized_(false), diff --git a/src/utils/load_gnss.cpp b/src/utils/load_gnss.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5bda1d4e9a0f02de63e3298120599da606fa37c5 --- /dev/null +++ b/src/utils/load_gnss.cpp @@ -0,0 +1,26 @@ +// 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/>. + +#include "gnss/utils/load_gnss.h" + +namespace wolf +{ +bool LoadGnss::aux_var = true; +} // namespace wolf diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index b09c6444079620200f9ad17a0a9c2de95ae3aafd..b590e8c33f434914b56934556ea9c2e2cecdda5d 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -11,26 +11,26 @@ add_subdirectory(gtest) # FactorGnssFix2d test wolf_add_gtest(gtest_factor_gnss_fix_2d gtest_factor_gnss_fix_2d.cpp) -# # FactorGnssFix3d test -# wolf_add_gtest(gtest_factor_gnss_fix_3d gtest_factor_gnss_fix_3d.cpp) +# FactorGnssFix3d test +wolf_add_gtest(gtest_factor_gnss_fix_3d gtest_factor_gnss_fix_3d.cpp) # FactorGnssFix3dWithClock test wolf_add_gtest(gtest_factor_gnss_fix_3d_with_clock gtest_factor_gnss_fix_3d_with_clock.cpp) -# # FactorGnssPseudoRange test -# wolf_add_gtest(gtest_factor_gnss_pseudo_range gtest_factor_gnss_pseudo_range.cpp) +# FactorGnssPseudoRange test +wolf_add_gtest(gtest_factor_gnss_pseudo_range gtest_factor_gnss_pseudo_range.cpp) -# # FactorGnssDisplacement2d test -# wolf_add_gtest(gtest_factor_gnss_displacement_2d gtest_factor_gnss_displacement_2d.cpp) +# FactorGnssDisplacement2d test +wolf_add_gtest(gtest_factor_gnss_displacement_2d gtest_factor_gnss_displacement_2d.cpp) -# # FactorGnssDisplacement3d test -# wolf_add_gtest(gtest_factor_gnss_displacement_3d gtest_factor_gnss_displacement_3d.cpp) +# FactorGnssDisplacement3d test +wolf_add_gtest(gtest_factor_gnss_displacement_3d gtest_factor_gnss_displacement_3d.cpp) -# # FactorGnssDisplacement3dWithClock test -# wolf_add_gtest(gtest_factor_gnss_displacement_3d_with_clock gtest_factor_gnss_displacement_3d_with_clock.cpp) +# FactorGnssDisplacement3dWithClock test +wolf_add_gtest(gtest_factor_gnss_displacement_3d_with_clock gtest_factor_gnss_displacement_3d_with_clock.cpp) -# # FactorGnssTdcp test -# wolf_add_gtest(gtest_factor_gnss_tdcp gtest_factor_gnss_tdcp.cpp) +# FactorGnssTdcp test +wolf_add_gtest(gtest_factor_gnss_tdcp gtest_factor_gnss_tdcp.cpp) # Schema test wolf_add_gtest(gtest_schema gtest_schema.cpp) diff --git a/test/gtest_factor_gnss_displacement_2d.cpp b/test/gtest_factor_gnss_displacement_2d.cpp index dd471c34325c10af4e93e994e42b6f7e09efd6ba..af29ff1090305e9956ad4b2a66efa8d85947c085 100644 --- a/test/gtest_factor_gnss_displacement_2d.cpp +++ b/test/gtest_factor_gnss_displacement_2d.cpp @@ -17,25 +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/>. -// WOLF - Copyright (C) 2020,2021,2022 -// 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/>. #include "gnss/factor/factor_gnss_displacement_2d.h" @@ -53,7 +34,8 @@ using namespace Eigen; using namespace wolf; -std::string gnss_dir = _WOLF_GNSS_CODE_DIR; +std::string gnss_dir = _WOLF_GNSS_CODE_DIR; +std::string wolf_schema_dir = _WOLF_SCHEMA_DIR; int N = 10; @@ -168,52 +150,15 @@ class FactorGnssDisplacement2dTest : public testing::Test void SetUp() override { - // WOLF - problem = Problem::create("PO", 2); - solver_ceres = std::make_shared<SolverCeres>(problem); - solver_ceres->getSolverOptions().max_num_iterations = 100; - solver_ceres->getSolverOptions().gradient_tolerance = 1e-8; - solver_ceres->getSolverOptions().function_tolerance = 1e-8; - - // sensor - YAML::Node param; - param["name"] = "a not so cool sensor"; - param["ENU_mode"] = "auto"; - - param["states"]["P"]["type"] = "StatePoint3d"; - param["states"]["P"]["state"] = t_base_antena; - param["states"]["P"]["mode"] = "fix"; - param["states"]["P"]["dynamic"] = false; - param["states"]["t"]["type"] = "StatePoint3d"; - param["states"]["t"]["state"] = t_enu_map; - param["states"]["t"]["mode"] = "fix"; - param["states"]["t"]["dynamic"] = false; - param["states"]["r"]["type"] = "StateAngle"; - param["states"]["r"]["state"] = Vector1d(rpy_enu_map(0)); - param["states"]["r"]["mode"] = "fix"; - param["states"]["r"]["dynamic"] = false; - param["states"]["p"]["type"] = "StateAngle"; - param["states"]["p"]["state"] = Vector1d(rpy_enu_map(1)); - param["states"]["p"]["mode"] = "fix"; - param["states"]["p"]["dynamic"] = false; - param["states"]["y"]["type"] = "StateAngle"; - param["states"]["y"]["state"] = Vector1d(rpy_enu_map(2)); - param["states"]["y"]["mode"] = "fix"; - param["states"]["y"]["dynamic"] = false; - param["states"]["T"]["type"] = "StateParams1"; - param["states"]["T"]["state"] = Vector1d::Zero(); - param["states"]["T"]["mode"] = "fix"; - param["states"]["T"]["dynamic"] = true; - param["states"]["G"]["type"] = "StateParams1"; - param["states"]["G"]["state"] = Vector1d::Zero(); - param["states"]["G"]["mode"] = "fix"; - param["states"]["G"]["dynamic"] = true; - param["states"]["E"]["type"] = "StateParams1"; - param["states"]["E"]["state"] = Vector1d::Zero(); - param["states"]["E"]["mode"] = "fix"; - param["states"]["E"]["dynamic"] = true; - - gnss_sensor = SensorBase::emplace<SensorGnss>(problem->getHardware(), param); + // Problem + problem = Problem::autoSetup(gnss_dir + "/test/yaml/problem_2d.yaml", {gnss_dir}); + + // Sensor + gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->findSensor("gnss receiver 1")); + + // Solver + solver_ceres = std::static_pointer_cast<SolverCeres>(FactorySolverFile::create( + "SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir})); // frames frame_1 = problem->emplaceFrame(TimeStamp(0), "PO", Vector3d::Zero()); diff --git a/test/gtest_factor_gnss_displacement_3d.cpp b/test/gtest_factor_gnss_displacement_3d.cpp index d73b627f696ccecbcfc07da9a72899a5a9edcbd7..5ea8d1f54cf869bf6c82b76bdb5f7e11762971e0 100644 --- a/test/gtest_factor_gnss_displacement_3d.cpp +++ b/test/gtest_factor_gnss_displacement_3d.cpp @@ -17,25 +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/>. -// WOLF - Copyright (C) 2020,2021,2022 -// 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/>. #include "gnss/factor/factor_gnss_displacement_3d.h" @@ -53,7 +34,8 @@ using namespace Eigen; using namespace wolf; -std::string gnss_dir = _WOLF_GNSS_CODE_DIR; +std::string gnss_dir = _WOLF_GNSS_CODE_DIR; +std::string wolf_schema_dir = _WOLF_SCHEMA_DIR; int N = 10; @@ -165,59 +147,26 @@ class FactorGnssDisplacement3dTest : public testing::Test void SetUp() override { - // WOLF - problem = Problem::create("PO", 3); - solver_ceres = std::make_shared<SolverCeres>(problem); - solver_ceres->getSolverOptions().max_num_iterations = 100; - solver_ceres->getSolverOptions().gradient_tolerance = 1e-8; - solver_ceres->getSolverOptions().function_tolerance = 1e-8; - - // sensor - YAML::Node param; - param["name"] = "a not so cool sensor"; - param["ENU_mode"] = "auto"; - - param["states"]["P"]["type"] = "StatePoint3d"; - param["states"]["P"]["state"] = t_base_antena; - param["states"]["P"]["mode"] = "fix"; - param["states"]["P"]["dynamic"] = false; - param["states"]["t"]["type"] = "StatePoint3d"; - param["states"]["t"]["state"] = t_enu_map; - param["states"]["t"]["mode"] = "fix"; - param["states"]["t"]["dynamic"] = false; - param["states"]["r"]["type"] = "StateAngle"; - param["states"]["r"]["state"] = Vector1d(rpy_enu_map(0)); - param["states"]["r"]["mode"] = "fix"; - param["states"]["r"]["dynamic"] = false; - param["states"]["p"]["type"] = "StateAngle"; - param["states"]["p"]["state"] = Vector1d(rpy_enu_map(1)); - param["states"]["p"]["mode"] = "fix"; - param["states"]["p"]["dynamic"] = false; - param["states"]["y"]["type"] = "StateAngle"; - param["states"]["y"]["state"] = Vector1d(rpy_enu_map(2)); - param["states"]["y"]["mode"] = "fix"; - param["states"]["y"]["dynamic"] = false; - param["states"]["T"]["type"] = "StateParams1"; - param["states"]["T"]["state"] = Vector1d::Zero(); - param["states"]["T"]["mode"] = "fix"; - param["states"]["T"]["dynamic"] = true; - param["states"]["G"]["type"] = "StateParams1"; - param["states"]["G"]["state"] = Vector1d::Zero(); - param["states"]["G"]["mode"] = "fix"; - param["states"]["G"]["dynamic"] = true; - param["states"]["E"]["type"] = "StateParams1"; - param["states"]["E"]["state"] = Vector1d::Zero(); - param["states"]["E"]["mode"] = "fix"; - param["states"]["E"]["dynamic"] = true; - - gnss_sensor = SensorBase::emplace<SensorGnss>(problem->getHardware(), param); + // Problem + problem = Problem::autoSetup(gnss_dir + "/test/yaml/problem_3d.yaml", {gnss_dir}); + + // Sensor + gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->findSensor("gnss receiver 1")); + + // Solver + solver_ceres = std::static_pointer_cast<SolverCeres>(FactorySolverFile::create( + "SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir})); // frames frame_1 = problem->emplaceFrame(TimeStamp(0), "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); + WOLF_INFO("frame 1 created") problem->keyFrameCallback(frame_1, nullptr); + WOLF_INFO("frame 1 informed") frame_2 = problem->emplaceFrame(TimeStamp(1), "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); + WOLF_INFO("frame 2 created") problem->keyFrameCallback(frame_2, nullptr); - + WOLF_INFO("frame 2 informed") + // captures cap_gnss_1 = CaptureBase::emplace<CaptureGnss>(frame_1, TimeStamp(0), gnss_sensor, nullptr); cap_gnss_2 = CaptureBase::emplace<CaptureGnss>(frame_2, TimeStamp(1), gnss_sensor, nullptr); diff --git a/test/gtest_factor_gnss_displacement_3d_with_clock.cpp b/test/gtest_factor_gnss_displacement_3d_with_clock.cpp index c88049774347b988e8362bbff1b60307c094db7e..368c0cff957bc9d80d8563968c45a3f238f1c6dd 100644 --- a/test/gtest_factor_gnss_displacement_3d_with_clock.cpp +++ b/test/gtest_factor_gnss_displacement_3d_with_clock.cpp @@ -17,25 +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/>. -// WOLF - Copyright (C) 2020,2021,2022 -// 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/>. #include "gnss/factor/factor_gnss_displacement_3d_with_clock.h" @@ -53,7 +34,8 @@ using namespace Eigen; using namespace wolf; -std::string gnss_dir = _WOLF_GNSS_CODE_DIR; +std::string gnss_dir = _WOLF_GNSS_CODE_DIR; +std::string wolf_schema_dir = _WOLF_SCHEMA_DIR; int N = 10; @@ -166,58 +148,25 @@ class FactorGnssDisplacement3dWithClockTest : public testing::Test void SetUp() override { - // WOLF - problem = Problem::create("PO", 3); - solver_ceres = std::make_shared<SolverCeres>(problem); - solver_ceres->getSolverOptions().max_num_iterations = 100; - solver_ceres->getSolverOptions().gradient_tolerance = 1e-8; - solver_ceres->getSolverOptions().function_tolerance = 1e-8; - - // sensor - YAML::Node param; - param["name"] = "a not so cool sensor"; - param["ENU_mode"] = "auto"; - - param["states"]["P"]["type"] = "StatePoint3d"; - param["states"]["P"]["state"] = t_base_antena; - param["states"]["P"]["mode"] = "fix"; - param["states"]["P"]["dynamic"] = false; - param["states"]["t"]["type"] = "StatePoint3d"; - param["states"]["t"]["state"] = t_enu_map; - param["states"]["t"]["mode"] = "fix"; - param["states"]["t"]["dynamic"] = false; - param["states"]["r"]["type"] = "StateAngle"; - param["states"]["r"]["state"] = Vector1d(rpy_enu_map(0)); - param["states"]["r"]["mode"] = "fix"; - param["states"]["r"]["dynamic"] = false; - param["states"]["p"]["type"] = "StateAngle"; - param["states"]["p"]["state"] = Vector1d(rpy_enu_map(1)); - param["states"]["p"]["mode"] = "fix"; - param["states"]["p"]["dynamic"] = false; - param["states"]["y"]["type"] = "StateAngle"; - param["states"]["y"]["state"] = Vector1d(rpy_enu_map(2)); - param["states"]["y"]["mode"] = "fix"; - param["states"]["y"]["dynamic"] = false; - param["states"]["T"]["type"] = "StateParams1"; - param["states"]["T"]["state"] = Vector1d::Zero(); - param["states"]["T"]["mode"] = "fix"; - param["states"]["T"]["dynamic"] = true; - param["states"]["G"]["type"] = "StateParams1"; - param["states"]["G"]["state"] = Vector1d::Zero(); - param["states"]["G"]["mode"] = "fix"; - param["states"]["G"]["dynamic"] = true; - param["states"]["E"]["type"] = "StateParams1"; - param["states"]["E"]["state"] = Vector1d::Zero(); - param["states"]["E"]["mode"] = "fix"; - param["states"]["E"]["dynamic"] = true; - - gnss_sensor = SensorBase::emplace<SensorGnss>(problem->getHardware(), param); + // Problem + problem = Problem::autoSetup(gnss_dir + "/test/yaml/problem_3d.yaml", {gnss_dir}); + + // Sensor + gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->findSensor("gnss receiver 1")); + + // Solver + solver_ceres = std::static_pointer_cast<SolverCeres>(FactorySolverFile::create( + "SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir})); // frames frame_1 = problem->emplaceFrame(TimeStamp(0), "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); + WOLF_INFO("frame 1 created") problem->keyFrameCallback(frame_1, nullptr); + WOLF_INFO("frame 1 informed") frame_2 = problem->emplaceFrame(TimeStamp(1), "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished()); + WOLF_INFO("frame 2 created") problem->keyFrameCallback(frame_2, nullptr); + WOLF_INFO("frame 2 informed") // captures cap_gnss_1 = CaptureBase::emplace<CaptureGnss>(frame_1, TimeStamp(0), gnss_sensor, nullptr); diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp index 8885c46633d787e9d4933c4cbb3f7e9cbc5aac58..2b2e488883edd772420b13a2501d0f8894b953cf 100644 --- a/test/gtest_factor_gnss_fix_2d.cpp +++ b/test/gtest_factor_gnss_fix_2d.cpp @@ -156,39 +156,16 @@ class FactorGnssFix2dTest : public testing::Test void SetUp() override { - // WOLF - problem = Problem::create("PO", 2); + // Problem + problem = Problem::autoSetup(gnss_dir + "/test/yaml/problem_2d.yaml", {gnss_dir}); + + // Sensor + gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->findSensor("gnss receiver 1")); + + // Solver solver_ceres = std::static_pointer_cast<SolverCeres>(FactorySolverFile::create( "SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir})); - // sensor - YAML::Node param; - param["name"] = "a not so cool sensor"; - param["type"] = "SensorGnss"; - param["plugin"] = "gnss"; - param["ENU"]["mode"] = "auto"; - - param["states"]["P"]["state"] = t_base_antena; - param["states"]["P"]["mode"] = "fix"; - param["states"]["P"]["dynamic"] = false; - param["states"]["t"]["state"] = t_enu_map; - param["states"]["t"]["mode"] = "fix"; - param["states"]["t"]["dynamic"] = false; - param["states"]["r"]["state"] = Vector1d(rpy_enu_map(0)); - param["states"]["r"]["mode"] = "fix"; - param["states"]["p"]["state"] = Vector1d(rpy_enu_map(1)); - param["states"]["p"]["mode"] = "fix"; - param["states"]["y"]["state"] = Vector1d(rpy_enu_map(2)); - param["states"]["y"]["mode"] = "fix"; - param["states"]["T"]["state"] = Vector1d::Zero(); - param["states"]["T"]["mode"] = "initial_guess"; - param["states"]["G"]["state"] = Vector1d::Zero(); - param["states"]["G"]["mode"] = "initial_guess"; - param["states"]["E"]["state"] = Vector1d::Zero(); - param["states"]["E"]["mode"] = "initial_guess"; - - gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->installSensor(param, {gnss_dir, wolf_schema_dir})); - // frame frame = problem->emplaceFrame(TimeStamp(0), "PO", Vector3d::Zero()); problem->keyFrameCallback(frame, nullptr); diff --git a/test/gtest_factor_gnss_fix_3d.cpp b/test/gtest_factor_gnss_fix_3d.cpp index a8d7626dce9cc405b153572871e2dec8f5ff3885..e0ea64f99b4fd4677abccd7131f301ef24b76ba7 100644 --- a/test/gtest_factor_gnss_fix_3d.cpp +++ b/test/gtest_factor_gnss_fix_3d.cpp @@ -37,6 +37,7 @@ using namespace wolf; int N = 10; std::string gnss_dir = _WOLF_GNSS_CODE_DIR; +std::string wolf_schema_dir = _WOLF_SCHEMA_DIR; class FactorGnssFix3dTest : public testing::Test { @@ -128,50 +129,15 @@ class FactorGnssFix3dTest : public testing::Test void SetUp() override { - // WOLF - problem = Problem::create("PO", 3); - solver_ceres = FactorySolverFile::create( - "SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml");//, {wolf_schema_dir}); - - // sensor - YAML::Node param; - param["name"] = "a not so cool sensor"; - param["ENU_mode"] = "auto"; - - param["states"]["P"]["type"] = "StatePoint3d"; - param["states"]["P"]["state"] = t_base_antena; - param["states"]["P"]["mode"] = "fix"; - param["states"]["P"]["dynamic"] = false; - param["states"]["t"]["type"] = "StatePoint3d"; - param["states"]["t"]["state"] = t_enu_map; - param["states"]["t"]["mode"] = "fix"; - param["states"]["t"]["dynamic"] = false; - param["states"]["r"]["type"] = "StateAngle"; - param["states"]["r"]["state"] = Vector1d(rpy_enu_map(0)); - param["states"]["r"]["mode"] = "fix"; - param["states"]["r"]["dynamic"] = false; - param["states"]["p"]["type"] = "StateAngle"; - param["states"]["p"]["state"] = Vector1d(rpy_enu_map(1)); - param["states"]["p"]["mode"] = "fix"; - param["states"]["p"]["dynamic"] = false; - param["states"]["y"]["type"] = "StateAngle"; - param["states"]["y"]["state"] = Vector1d(rpy_enu_map(2)); - param["states"]["y"]["mode"] = "fix"; - param["states"]["y"]["dynamic"] = false; - param["states"]["T"]["type"] = "StateParams1"; - param["states"]["T"]["state"] = Vector1d::Zero(); - param["states"]["T"]["mode"] = "fix"; - param["states"]["T"]["dynamic"] = true; - param["states"]["G"]["type"] = "StateParams1"; - param["states"]["G"]["state"] = Vector1d::Zero(); - param["states"]["G"]["mode"] = "fix"; - param["states"]["G"]["dynamic"] = true; - param["states"]["E"]["type"] = "StateParams1"; - param["states"]["E"]["state"] = Vector1d::Zero(); - param["states"]["E"]["mode"] = "fix"; - param["states"]["E"]["dynamic"] = true; - - gnss_sensor = SensorBase::emplace<SensorGnss>(problem->getHardware(), param, true); + // Problem + problem = Problem::autoSetup(gnss_dir + "/test/yaml/problem_3d.yaml", {gnss_dir}); + + // Sensor + gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->findSensor("gnss receiver 1")); + + // Solver + solver_ceres = std::static_pointer_cast<SolverCeres>(FactorySolverFile::create( + "SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir})); // frame frame = problem->emplaceFrame(TimeStamp(0)); diff --git a/test/gtest_factor_gnss_fix_3d_with_clock.cpp b/test/gtest_factor_gnss_fix_3d_with_clock.cpp index d9184530ec58aae56cfc0361193388df90a041fe..1bc863a038e3c7ffba37a2f6b7f89a5906e35af1 100644 --- a/test/gtest_factor_gnss_fix_3d_with_clock.cpp +++ b/test/gtest_factor_gnss_fix_3d_with_clock.cpp @@ -133,55 +133,16 @@ class FactorGnssFix3dWithClockTest : public testing::Test void SetUp() override { - // WOLF - problem = Problem::create("PO", 3); - solver_ceres = std::static_pointer_cast<SolverCeres>(FactorySolverFile::create( - "SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir})); + // Problem + problem = Problem::autoSetup(gnss_dir + "/test/yaml/problem_3d.yaml", {gnss_dir}); - // sensor - YAML::Node param; - param["name"] = "a not so cool sensor"; - param["type"] = "SensorGnss"; - param["plugin"] = "gnss"; - param["name"] = "a not so cool sensor"; - param["ENU"]["mode"] = "auto"; - - param["states"]["P"]["type"] = "StatePoint3d"; - param["states"]["P"]["state"] = t_base_antena; - param["states"]["P"]["mode"] = "fix"; - param["states"]["P"]["dynamic"] = false; - param["states"]["t"]["type"] = "StatePoint3d"; - param["states"]["t"]["state"] = t_enu_map; - param["states"]["t"]["mode"] = "fix"; - param["states"]["t"]["dynamic"] = false; - param["states"]["r"]["type"] = "StateAngle"; - param["states"]["r"]["state"] = Vector1d(rpy_enu_map(0)); - param["states"]["r"]["mode"] = "fix"; - param["states"]["r"]["dynamic"] = false; - param["states"]["p"]["type"] = "StateAngle"; - param["states"]["p"]["state"] = Vector1d(rpy_enu_map(1)); - param["states"]["p"]["mode"] = "fix"; - param["states"]["p"]["dynamic"] = false; - param["states"]["y"]["type"] = "StateAngle"; - param["states"]["y"]["state"] = Vector1d(rpy_enu_map(2)); - param["states"]["y"]["mode"] = "fix"; - param["states"]["y"]["dynamic"] = false; - param["states"]["T"]["type"] = "StateParams1"; - param["states"]["T"]["state"] = Vector1d::Zero(); - param["states"]["T"]["mode"] = "initial_guess"; - param["states"]["T"]["dynamic"] = true; - param["states"]["G"]["type"] = "StateParams1"; - param["states"]["G"]["state"] = Vector1d::Zero(); - param["states"]["G"]["mode"] = "initial_guess"; - param["states"]["G"]["dynamic"] = true; - param["states"]["E"]["type"] = "StateParams1"; - param["states"]["E"]["state"] = Vector1d::Zero(); - param["states"]["E"]["mode"] = "initial_guess"; - param["states"]["E"]["dynamic"] = true; - - gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->installSensor(param, {gnss_dir, wolf_schema_dir})); - problem->installSensor(wolf_dir + "/test/yaml/sensor_odom_3d.yaml", {wolf_schema_dir}); + // Sensor + gnss_sensor = std::static_pointer_cast<SensorGnss>(problem->findSensor("gnss receiver 1")); + // Solver + solver_ceres = std::static_pointer_cast<SolverCeres>(FactorySolverFile::create( + "SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir})); + // frame frame = problem->emplaceFrame(TimeStamp(0)); problem->keyFrameCallback(frame, nullptr); diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp index bbf42f9dc92daf67a9e18d0341c42472fc2b8a91..a91eb757f21a0e43ba84c6a601972055282c0913 100644 --- a/test/gtest_factor_gnss_pseudo_range.cpp +++ b/test/gtest_factor_gnss_pseudo_range.cpp @@ -65,13 +65,15 @@ class FactorGnssPreusoRangeTest : public testing::Test sat3 = Satellite({SYS_GPS, 1, Vector3d::Zero(), Vector3d::Zero(), 0, 0, 0}); sat4 = Satellite({SYS_GPS, 1, Vector3d::Zero(), Vector3d::Zero(), 0, 0, 0}); - // WOLF - prb = Problem::create("PO", 3); + // Problem + prb = Problem::autoSetup(gnss_dir + "/test/yaml/problem_3d.yaml", {gnss_dir}); + + // Sensor + gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->findSensor("gnss receiver 1")); + + // Solver solver = FactorySolverFile::create( "SolverCeres", prb, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir}); - - gnss_sensor = std::static_pointer_cast<SensorGnss>( - prb->installSensor(gnss_dir + "/test/yaml/sensor_gnss.yaml", {gnss_dir, wolf_schema_dir})); } void setUpRandom() diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp index cb6ffdf574ca30ca6a6754fada1400f0401fac5d..dd95c39b1188cd15b6d8a31169738b50ac5e38f3 100644 --- a/test/gtest_factor_gnss_tdcp.cpp +++ b/test/gtest_factor_gnss_tdcp.cpp @@ -71,12 +71,15 @@ class FactorGnssTdcpTest : public testing::Test sat3_k = Satellite({SYS_GPS, 1, Vector3d::Zero(), Vector3d::Zero(), 0, 0, 0}); sat4_k = Satellite({SYS_GPS, 1, Vector3d::Zero(), Vector3d::Zero(), 0, 0, 0}); - // WOLF - prb = Problem::create("PO", 3); + // Problem + prb = Problem::autoSetup(gnss_dir + "/test/yaml/problem_3d.yaml", {gnss_dir}); + + // Sensor + gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->findSensor("gnss receiver 1")); + + // Solver solver = FactorySolverFile::create( "SolverCeres", prb, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir}); - gnss_sensor = std::static_pointer_cast<SensorGnss>( - prb->installSensor(gnss_dir + "/test/yaml/sensor_gnss.yaml", {gnss_dir, wolf_schema_dir})); } void setUpRandom() diff --git a/test/gtest_schema.cpp b/test/gtest_schema.cpp index 59267e88b6b018bea16c9e08d7aa88ab91191ab4..37e98997f91a46becc8c60a4b9e763ccb7c7d311 100644 --- a/test/gtest_schema.cpp +++ b/test/gtest_schema.cpp @@ -20,11 +20,13 @@ #include "gnss/common/gnss.h" #include "core/utils/utils_gtest.h" -#include "core/map/factory_map.h" -#include "core/sensor/factory_sensor.h" -#include "core/solver/factory_solver.h" -#include "core/processor/factory_processor.h" -#include "core/tree_manager/factory_tree_manager.h" +#include "gnss/capture/capture_gnss_fix.h" +#include "gnss/sensor/sensor_gnss.h" +#include "core/map/map_base.h" +#include "core/sensor/sensor_base.h" +#include "core/solver/solver_manager.h" +#include "core/processor/processor_base.h" +#include "core/tree_manager/tree_manager_base.h" #include "yaml-schema-cpp/yaml_schema.hpp" @@ -36,8 +38,8 @@ std::string wolf_schema_dir = _WOLF_SCHEMA_DIR; std::string gnss_dir = _WOLF_GNSS_CODE_DIR; // just force the plugin .so library to be loaded -auto cap_gnss = - std::make_shared<CaptureGnssFix>(TimeStamp(), nullptr, Eigen::Vector4d::Zero(), Eigen::Matrix4d::Zero()); +WOLF_LOAD_CORE; +WOLF_LOAD_GNSS; bool existsSchema(std::string name_schema) { diff --git a/test/yaml/problem_2d.yaml b/test/yaml/problem_2d.yaml new file mode 100644 index 0000000000000000000000000000000000000000..273a0cc3fcf95af1685a105907ff94fe6516bece --- /dev/null +++ b/test/yaml/problem_2d.yaml @@ -0,0 +1,20 @@ +problem: + dimension: 2 + first_frame: + P: + state: [0,0] + mode: "initial_guess" + O: + state: [0] + mode: "initial_guess" + tree_manager: + type: "None" + +map: + type: "MapBase" + plugin: "core" + +sensors: + - follow: sensor_gnss.yaml + +processors: [] \ No newline at end of file diff --git a/test/yaml/problem_3d.yaml b/test/yaml/problem_3d.yaml new file mode 100644 index 0000000000000000000000000000000000000000..c02bcb27d1df92fae1c3449d9642a0de6f7d1b9d --- /dev/null +++ b/test/yaml/problem_3d.yaml @@ -0,0 +1,21 @@ +problem: + dimension: 3 + first_frame: + P: + state: [0,0,0] + mode: "initial_guess" + O: + state: [0,0,0,1] + mode: "initial_guess" + tree_manager: + type: "None" + +map: + type: "MapBase" + plugin: "core" + +sensors: + - + follow: sensor_gnss.yaml + +processors: [] \ No newline at end of file