From c5552c55334dec66487b483151fe952179d57d76 Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Sun, 2 Jun 2024 10:29:01 +0200 Subject: [PATCH 1/4] Refactor Simulator Config Add base class for the Simulator::Config to be used in 2D and 3D subclasses. --- g2o/simulator/simulator.h | 12 +++++++++++ g2o/simulator/simulator2d_base.h | 11 +---------- g2o/simulator/simulator3d_base.h | 10 +--------- python/simulator/py_simulator.cpp | 33 +++++++++++++------------------ 4 files changed, 28 insertions(+), 38 deletions(-) diff --git a/g2o/simulator/simulator.h b/g2o/simulator/simulator.h index 539a000bd..6dc75d6b8 100644 --- a/g2o/simulator/simulator.h +++ b/g2o/simulator/simulator.h @@ -289,6 +289,18 @@ class BinarySensor : public BaseSensor { */ class Simulator { public: + struct Config { + double worldSize = 25.; + int nlandmarks = 0; + int simSteps = 100; + bool hasOdom = false; + // Poses and landmarks + bool hasPoseSensor = false; + bool hasPointSensor = false; + bool hasCompass = false; + bool hasGPS = false; + }; + virtual ~Simulator() = default; virtual void setup() = 0; virtual void simulate() = 0; diff --git a/g2o/simulator/simulator2d_base.h b/g2o/simulator/simulator2d_base.h index 42f4da4b3..e2edac345 100644 --- a/g2o/simulator/simulator2d_base.h +++ b/g2o/simulator/simulator2d_base.h @@ -51,16 +51,7 @@ class G2O_SIMULATOR_API Simulator2D : public Simulator { /** * @brief Configuration of the 2D simulator */ - struct Config { - double worldSize = 25.; - int nlandmarks = 0; - int simSteps = 100; - bool hasOdom = false; - // Poses and landmarks - bool hasPoseSensor = false; - bool hasPointSensor = false; - bool hasCompass = false; - bool hasGPS = false; + struct Config : public Simulator::Config { bool hasPointBearingSensor = false; // Segment diff --git a/g2o/simulator/simulator3d_base.h b/g2o/simulator/simulator3d_base.h index 734ff348c..1af9fb11a 100644 --- a/g2o/simulator/simulator3d_base.h +++ b/g2o/simulator/simulator3d_base.h @@ -50,17 +50,9 @@ class G2O_SIMULATOR_API Simulator3D : public Simulator { /** * @brief Configuration of the 3D simulator */ - struct Config { - double worldSize = 25.; - int nlandmarks = 0; - int simSteps = 100; - bool hasOdom = false; - bool hasPoseSensor = false; - bool hasPointSensor = false; + struct Config : public Simulator::Config { bool hasPointDepthSensor = false; bool hasPointDisparitySensor = false; - bool hasCompass = false; - bool hasGPS = false; }; Simulator3D() = default; diff --git a/python/simulator/py_simulator.cpp b/python/simulator/py_simulator.cpp index be56e3ced..3be83869b 100644 --- a/python/simulator/py_simulator.cpp +++ b/python/simulator/py_simulator.cpp @@ -24,17 +24,20 @@ void declareSimulator(py::module& m) { static_cast(&Simulator::world), py::return_value_policy::reference); + py::class_(m, "SimulatorConfig") + .def(py::init<>()) + .def_readwrite("world_size", &Simulator::Config::worldSize) + .def_readwrite("nlandmarks", &Simulator::Config::nlandmarks) + .def_readwrite("sim_steps", &Simulator::Config::simSteps) + .def_readwrite("has_odom", &Simulator::Config::hasOdom) + .def_readwrite("has_pose_sensor", &Simulator::Config::hasPoseSensor) + .def_readwrite("has_point_sensor", &Simulator::Config::hasPointSensor) + .def_readwrite("has_compass", &Simulator::Config::hasCompass) + .def_readwrite("has_gps", &Simulator::Config::hasGPS); + // 2D Simulator - py::class_(m, "Simulator2DConfig") + py::class_(m, "Simulator2DConfig") .def(py::init<>()) - .def_readwrite("world_size", &Simulator2D::Config::worldSize) - .def_readwrite("nlandmarks", &Simulator2D::Config::nlandmarks) - .def_readwrite("sim_steps", &Simulator2D::Config::simSteps) - .def_readwrite("has_odom", &Simulator2D::Config::hasOdom) - .def_readwrite("has_pose_sensor", &Simulator2D::Config::hasPoseSensor) - .def_readwrite("has_point_sensor", &Simulator2D::Config::hasPointSensor) - .def_readwrite("has_compass", &Simulator2D::Config::hasCompass) - .def_readwrite("has_gps", &Simulator2D::Config::hasGPS) .def_readwrite("has_point_bearing_sensor", &Simulator2D::Config::hasPointBearingSensor) .def_readwrite("has_segment_sensor", @@ -56,20 +59,12 @@ void declareSimulator(py::module& m) { .def("simulate", &Simulator2D::simulate); // 3D Simulator - py::class_(m, "Simulator3DConfig") + py::class_(m, "Simulator3DConfig") .def(py::init<>()) - .def_readwrite("world_size", &Simulator3D::Config::worldSize) - .def_readwrite("nlandmarks", &Simulator3D::Config::nlandmarks) - .def_readwrite("sim_steps", &Simulator3D::Config::simSteps) - .def_readwrite("has_odom", &Simulator3D::Config::hasOdom) - .def_readwrite("has_pose_sensor", &Simulator3D::Config::hasPoseSensor) - .def_readwrite("has_point_sensor", &Simulator3D::Config::hasPointSensor) .def_readwrite("has_point_depth_sensor", &Simulator3D::Config::hasPointDepthSensor) .def_readwrite("has_point_disparity_sensor", - &Simulator3D::Config::hasPointDisparitySensor) - .def_readwrite("has_compass", &Simulator3D::Config::hasCompass) - .def_readwrite("has_gps", &Simulator3D::Config::hasGPS); + &Simulator3D::Config::hasPointDisparitySensor); py::class_(m, "Simulator3D") .def(py::init<>()) From 6b0d0854fa739c050ce4e2342bffec9425bd4f93 Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Sun, 2 Jun 2024 12:03:55 +0200 Subject: [PATCH 2/4] Do not copy Isometry3 data Most likely the compiler can optimize this but better not to ask for this. --- g2o/types/slam3d/edge_se3_prior.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/g2o/types/slam3d/edge_se3_prior.cpp b/g2o/types/slam3d/edge_se3_prior.cpp index bdc126758..e3bb9ce63 100644 --- a/g2o/types/slam3d/edge_se3_prior.cpp +++ b/g2o/types/slam3d/edge_se3_prior.cpp @@ -59,13 +59,10 @@ void EdgeSE3Prior::computeError() { void EdgeSE3Prior::linearizeOplus() { VertexSE3* from = vertexXnRaw<0>(); + const Isometry3& X = from->estimate(); + const Isometry3& P = cache_->offsetParam()->param(); + const Isometry3& Z = measurement_; Isometry3 E; - Isometry3 Z; - Isometry3 X; - Isometry3 P; - X = from->estimate(); - P = cache_->offsetParam()->param(); - Z = measurement_; internal::computeEdgeSE3PriorGradient(E, jacobianOplusXi_, Z, X, P); } From 0c20a4fae2a5709962d3b5abe6666c248002f4c4 Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Sun, 2 Jun 2024 12:06:47 +0200 Subject: [PATCH 3/4] Add 2D GPS to the simulator --- g2o/simulator/CMakeLists.txt | 1 + g2o/simulator/sensor_se2_prior.cpp | 60 +++++++++++++++++++++++++++++ g2o/simulator/sensor_se2_prior.h | 52 +++++++++++++++++++++++++ g2o/simulator/simulator2d_base.cpp | 14 ++++++- g2o/types/slam2d/edge_se2_prior.cpp | 5 +++ g2o/types/slam2d/edge_se2_prior.h | 2 + 6 files changed, 132 insertions(+), 2 deletions(-) create mode 100644 g2o/simulator/sensor_se2_prior.cpp create mode 100644 g2o/simulator/sensor_se2_prior.h diff --git a/g2o/simulator/CMakeLists.txt b/g2o/simulator/CMakeLists.txt index 27039e1d0..5e7f8d8a4 100644 --- a/g2o/simulator/CMakeLists.txt +++ b/g2o/simulator/CMakeLists.txt @@ -20,6 +20,7 @@ add_library(simulator_lib ${G2O_LIB_TYPE} sensor_pointxyz_disparity.cpp sensor_pointxyz_disparity.h sensor_pointxyz_depth.cpp sensor_pointxyz_depth.h sensor_se3_prior.cpp sensor_se3_prior.h + sensor_se2_prior.cpp sensor_se2_prior.h g2o_simulator_api.h ) diff --git a/g2o/simulator/sensor_se2_prior.cpp b/g2o/simulator/sensor_se2_prior.cpp new file mode 100644 index 000000000..83fd0190a --- /dev/null +++ b/g2o/simulator/sensor_se2_prior.cpp @@ -0,0 +1,60 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#include "sensor_se2_prior.h" + +#include + +#include "g2o/core/eigen_types.h" + +namespace g2o { + +// SensorSE2Prior +SensorSE2Prior::SensorSE2Prior(std::string name) + : UnarySensor(std::move(name)) { + information_ = Vector3(100., 100., 1000.).asDiagonal(); + setInformation(information_); +} + +void SensorSE2Prior::addNoise(EdgeType* e) { + EdgeType::ErrorVector _n = sampler_.generateSample(); + EdgeType::Measurement n(_n); + e->setMeasurement(e->measurement() * n); + e->setInformation(information()); +} + +void SensorSE2Prior::sense(BaseRobot& robot, World& world) { + robotPoseVertex_ = robotPoseVertex(robot, world); + if (!robotPoseVertex_) return; + sensorPose_ = robotPoseVertex_->estimate(); + auto e = mkEdge(); + if (!e) return; + world.graph().addEdge(e); + e->setMeasurementFromState(); + addNoise(e.get()); +} + +} // namespace g2o diff --git a/g2o/simulator/sensor_se2_prior.h b/g2o/simulator/sensor_se2_prior.h new file mode 100644 index 000000000..111b7054b --- /dev/null +++ b/g2o/simulator/sensor_se2_prior.h @@ -0,0 +1,52 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef G2O_SENSOR_SE2_PRIOR_H_ +#define G2O_SENSOR_SE2_PRIOR_H_ + +#include "g2o/types/slam2d/edge_se2_prior.h" +#include "g2o_simulator_api.h" +#include "pointsensorparameters.h" +#include "simulator2d_base.h" + +namespace g2o { + +class G2O_SIMULATOR_API SensorSE2Prior + : public PointSensorParameters, + public UnarySensor { + public: + using RobotPoseType = PoseVertexType::EstimateType; + explicit SensorSE2Prior(std::string name); + void sense(BaseRobot& robot, World& world) override; + void addNoise(EdgeType* e) override; + + protected: + RobotPoseType sensorPose_; +}; + +} // namespace g2o + +#endif diff --git a/g2o/simulator/simulator2d_base.cpp b/g2o/simulator/simulator2d_base.cpp index bc3e15334..a0cc0293a 100644 --- a/g2o/simulator/simulator2d_base.cpp +++ b/g2o/simulator/simulator2d_base.cpp @@ -35,6 +35,7 @@ #include "g2o/simulator/sensor_pointxy.h" #include "g2o/simulator/sensor_pointxy_bearing.h" #include "g2o/simulator/sensor_pose2d.h" +#include "g2o/simulator/sensor_se2_prior.h" #include "g2o/simulator/sensor_segment2d.h" #include "g2o/simulator/sensor_segment2d_line.h" #include "g2o/simulator/sensor_segment2d_pointline.h" @@ -85,6 +86,7 @@ void Simulator2D::setup() { auto robot = std::make_unique("myRobot"); if (config.hasOdom) { + G2O_DEBUG("Adding odometry sensor"); auto odometrySensor = std::make_unique("odometry"); const Vector3 diagonal(500, 500, 5000); odometrySensor->setInformation(diagonal.asDiagonal()); @@ -92,6 +94,7 @@ void Simulator2D::setup() { } if (config.hasPoseSensor) { + G2O_DEBUG("Adding pose sensor"); auto poseSensor = std::make_unique("poseSensor"); Matrix3 poseInfo = poseSensor->information(); poseInfo.setIdentity(); @@ -101,7 +104,14 @@ void Simulator2D::setup() { robot->addSensor(std::move(poseSensor), world_); } + if (config.hasGPS) { + G2O_DEBUG("Adding GPS sensor"); + auto gpsSensor = std::make_unique("gpsSensor"); + robot->addSensor(std::move(gpsSensor), world_); + } + if (config.hasPointSensor) { + G2O_DEBUG("Adding point sensor"); auto pointSensor = std::make_unique("pointSensor"); Matrix2 pointInfo = pointSensor->information(); pointInfo.setIdentity(); @@ -112,6 +122,7 @@ void Simulator2D::setup() { } if (config.hasPointBearingSensor) { + G2O_DEBUG("Adding point bearing sensor"); auto bearingSensor = std::make_unique("bearingSensor"); bearingSensor->setInformation(bearingSensor->information() * 1000); @@ -119,9 +130,8 @@ void Simulator2D::setup() { } if (config.hasSegmentSensor) { - G2O_TRACE("creating Segment Sensor"); + G2O_DEBUG("Adding segment sensor"); auto segmentSensor = std::make_unique("segmentSensor"); - G2O_TRACE("segmentSensorCreated"); segmentSensor->setMaxRange(3); segmentSensor->setMinRange(.1); segmentSensor->setInformation(segmentSensor->information() * 1000); diff --git a/g2o/types/slam2d/edge_se2_prior.cpp b/g2o/types/slam2d/edge_se2_prior.cpp index 8834d23cf..931b9aca3 100644 --- a/g2o/types/slam2d/edge_se2_prior.cpp +++ b/g2o/types/slam2d/edge_se2_prior.cpp @@ -45,4 +45,9 @@ void EdgeSE2Prior::setMeasurement(const SE2& m) { inverseMeasurement_ = m.inverse(); } +bool EdgeSE2Prior::setMeasurementFromState() { + setMeasurement(vertexXnRaw<0>()->estimate()); + return true; +} + } // namespace g2o diff --git a/g2o/types/slam2d/edge_se2_prior.h b/g2o/types/slam2d/edge_se2_prior.h index e993577ad..be6d229a6 100644 --- a/g2o/types/slam2d/edge_se2_prior.h +++ b/g2o/types/slam2d/edge_se2_prior.h @@ -58,6 +58,8 @@ class G2O_TYPES_SLAM2D_API EdgeSE2Prior void setMeasurement(const SE2& m) override; + bool setMeasurementFromState() override; + double initialEstimatePossible(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*) override { return 1.; From 2425d094a49d1fbfc4ac0bdb30338ef148705449 Mon Sep 17 00:00:00 2001 From: Rainer Kuemmerle Date: Sun, 2 Jun 2024 12:44:31 +0200 Subject: [PATCH 4/4] Extend tests for Simlator2D --- g2o/simulator/simulator2d_base.cpp | 3 + unit_test/simulator/simulator2D_tests.cpp | 151 ++++++++++++++++++++-- 2 files changed, 144 insertions(+), 10 deletions(-) diff --git a/g2o/simulator/simulator2d_base.cpp b/g2o/simulator/simulator2d_base.cpp index a0cc0293a..8d79d1260 100644 --- a/g2o/simulator/simulator2d_base.cpp +++ b/g2o/simulator/simulator2d_base.cpp @@ -165,6 +165,9 @@ void Simulator2D::simulate() { const Config& sim_conf = config; std::mt19937& generator = generator_; + G2O_DEBUG("Simulate {} steps in world size {}", sim_conf.simSteps, + sim_conf.worldSize); + for (const auto& robot : world_.robots()) { auto* rob2d = dynamic_cast(robot.get()); if (!rob2d) continue; diff --git a/unit_test/simulator/simulator2D_tests.cpp b/unit_test/simulator/simulator2D_tests.cpp index 92b826a84..88725353f 100644 --- a/unit_test/simulator/simulator2D_tests.cpp +++ b/unit_test/simulator/simulator2D_tests.cpp @@ -25,13 +25,57 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include +#include +#include "g2o/core/optimizable_graph.h" +#include "g2o/simulator/simulator.h" #include "g2o/simulator/simulator2d_base.h" #include "g2o/types/slam2d/edge_se2.h" +#include "g2o/types/slam2d/edge_se2_pointxy.h" +#include "g2o/types/slam2d/edge_se2_pointxy_bearing.h" +#include "g2o/types/slam2d/edge_se2_prior.h" +#include "g2o/types/slam2d/vertex_point_xy.h" +#include "g2o/types/slam2d/vertex_se2.h" +#include "g2o/types/slam2d_addons/edge_se2_segment2d.h" #include "gmock/gmock.h" +#include "gtest/gtest.h" using namespace g2o; // NOLINT using namespace testing; // NOLINT +namespace { +template +int countVerticesMatchingType(const OptimizableGraph& graph) { + return std::count_if( + graph.vertices().begin(), graph.vertices().end(), + [](const OptimizableGraph::VertexIDMap::value_type& elem) { + auto* ptr = dynamic_cast(elem.second.get()); + return ptr != nullptr; + }); +} + +template +int countEdgesMatchingType(const OptimizableGraph& graph) { + return std::count_if(graph.edges().begin(), graph.edges().end(), + [](const OptimizableGraph::EdgeSet::key_type& edge) { + auto* ptr = dynamic_cast(edge.get()); + return ptr != nullptr; + }); +} +} // namespace + +TEST(Simulator2D, Empty) { + Simulator2D simulator; + simulator.setup(); + simulator.simulate(); + + const auto& graph = simulator.world().graph(); + + EXPECT_THAT(graph.vertices(), SizeIs(0)); + EXPECT_THAT(graph.edges(), IsEmpty()); + EXPECT_THAT(graph.parameters(), SizeIs(0)); +} + TEST(Simulator2D, Odom) { Simulator2D simulator; simulator.config.hasOdom = true; @@ -39,16 +83,103 @@ TEST(Simulator2D, Odom) { simulator.setup(); simulator.simulate(); - const auto& graph = simulator.world().graph(); + const OptimizableGraph& graph = simulator.world().graph(); - EXPECT_THAT(simulator.graph().vertices(), - SizeIs(simulator.config.simSteps + 1)); - // count pose edges - const int odom_cnt = - std::count_if(graph.edges().begin(), graph.edges().end(), - [](const OptimizableGraph::EdgeSet::key_type& edge) { - auto* odom = dynamic_cast(edge.get()); - return odom; - }); + EXPECT_THAT(graph.vertices(), SizeIs(simulator.config.simSteps + 1)); + const int poses = countVerticesMatchingType(graph); + EXPECT_THAT(poses, Eq(simulator.config.simSteps + 1)); + const int odom_cnt = countEdgesMatchingType(graph); EXPECT_THAT(simulator.config.simSteps, Eq(odom_cnt)); } + +TEST(Simulator2D, NoLandmarks) { + Simulator2D simulator; + simulator.config.hasPointSensor = true; + + simulator.setup(); + simulator.simulate(); + + const OptimizableGraph& graph = simulator.world().graph(); + + EXPECT_THAT(graph.vertices(), SizeIs(0)); + EXPECT_THAT(graph.edges(), IsEmpty()); + EXPECT_THAT(graph.parameters(), SizeIs(0)); +} + +class Simulator2DTests : public ::testing::TestWithParam { + protected: + void SetUp() override { + simulator_.config = GetParam(); + simulator_.config.nlandmarks = std::max(100, simulator_.config.nlandmarks); + simulator_.config.nSegments = std::max(100, simulator_.config.nSegments); + } + + Simulator2D simulator_; +}; + +TEST_P(Simulator2DTests, Simulate) { + simulator_.setup(); + simulator_.simulate(); + + const OptimizableGraph& graph = simulator_.world().graph(); + const Simulator2D::Config& config = simulator_.config; + + if (config.hasOdom || config.hasPoseSensor || config.hasPointSensor || + config.hasPointBearingSensor || config.hasCompass || config.hasGPS) + EXPECT_THAT(countVerticesMatchingType(graph), Gt(0)); + + if (config.hasPointSensor || config.hasPointBearingSensor) + EXPECT_THAT(countVerticesMatchingType(graph), Gt(0)); + + // Base configuration + if (config.hasOdom || config.hasPoseSensor) + EXPECT_THAT(countEdgesMatchingType(graph), Gt(0)); + if (config.hasPointSensor) + EXPECT_THAT(countEdgesMatchingType(graph), Gt(0)); + + /* TODO(Rainer): Add simulation of a compass + if (config.hasCompass) + EXPECT_THAT(countEdgesMatchingType(graph), Gt(0)); + */ + if (config.hasGPS) + EXPECT_THAT(countEdgesMatchingType(graph), Gt(0)); + + // 2D specific configuration + if (config.hasPointBearingSensor) + EXPECT_THAT(countEdgesMatchingType(graph), Gt(0)); + if (config.hasSegmentSensor) + EXPECT_THAT(countEdgesMatchingType(graph), Gt(0)); +} + +namespace { +Simulator2D::Config FromWords(const std::unordered_set& words) { + Simulator2D::Config result; + result.hasOdom = words.count("hasOdom") != 0; + result.hasPoseSensor = words.count("hasPoseSensor") != 0; + result.hasPointSensor = words.count("hasPointSensor") != 0; + result.hasCompass = words.count("hasCompass") != 0; + result.hasGPS = words.count("hasGPS") != 0; + result.hasPointBearingSensor = words.count("hasPointBearingSensor") != 0; + result.hasSegmentSensor = words.count("hasSegmentSensor") != 0; + return result; +} + +std::vector ConfigsToTest() { + std::vector result; + // single sensors + result.push_back(FromWords({"hasOdom"})); + result.push_back(FromWords({"hasPoseSensor"})); + result.push_back(FromWords({"hasGPS"})); + result.push_back(FromWords({"hasPointSensor"})); + result.push_back(FromWords({"hasPointBearingSensor"})); + result.push_back(FromWords({"hasSegmentSensor"})); + // multiple + result.push_back(FromWords({"hasOdom", "hasPoseSensor"})); + result.push_back(FromWords({"hasOdom", "hasPoseSensor", "hasGps"})); + result.push_back(FromWords({"hasOdom", "hasPointSensor"})); + return result; +} +} // namespace + +INSTANTIATE_TEST_SUITE_P(Simulator, Simulator2DTests, + ValuesIn(ConfigsToTest()));