gtest implemented and passing

include/core/problem/problem.h

@@ -178,10 +178,8 @@ class Problem : public std::enable_shared_from_this<Problem>
 
     public:
         IsMotionPtr getProcessorIsMotion();
-        std::map<int,IsMotionPtr> getProcessorIsMotionMap();
-        
-
-
+        std::map<int,IsMotionPtr>& getProcessorIsMotionMap();
+        const std::map<int,IsMotionPtr>& getProcessorIsMotionMap() const;
 
         // Trajectory branch ----------------------------------
         TrajectoryBasePtr getTrajectory() const;
@@ -437,8 +435,12 @@ inline IsMotionPtr Problem::getProcessorIsMotion()
     return nullptr;
 }
 
+inline std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap()
+{
+    return processor_is_motion_map_;
+}
 
-inline std::map<int,IsMotionPtr> Problem::getProcessorIsMotionMap()
+inline const std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap() const
 {
     return processor_is_motion_map_;
 }

src/processor/is_motion.cpp

@@ -14,9 +14,11 @@ void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
     _prb_ptr->addProcessorIsMotion(_motion_ptr);
 }
 
-void IsMotion::setOdometry(const VectorComposite& _zero_odom)
+void IsMotion::setOdometry(const VectorComposite& _odom)
 {
+    assert(_odom.includesStructure(state_structure_) && "IsMotion::setOdometry(): provided odom has not any key.");
+
     for (auto key : state_structure_)
-        if (_zero_odom.count(key) != 0)
-            odometry_[key] = _zero_odom.at(key); //overwrite or insert only keys of the state_structure_
+        if (_odom.count(key) != 0)
+            odometry_[key] = _odom.at(key); //overwrite or insert only keys of the state_structure_
 }

test/CMakeLists.txt

@@ -84,6 +84,10 @@ target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
 wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
 target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
 
+# IsMotion classes test
+wolf_add_gtest(gtest_is_motion gtest_is_motion.cpp)
+target_link_libraries(gtest_is_motion ${PLUGIN_NAME})
+
 # LocalParametrizationXxx classes test
 wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
 target_link_libraries(gtest_local_param ${PLUGIN_NAME})

test/dummy/processor_is_motion_dummy.h

+/*
+ * processor_is_motion_dummy.h
+ *
+ *  Created on: Mar 8, 2021
+ *      Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
+#define TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
+
+#include "core/processor/processor_base.h"
+#include "core/processor/is_motion.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorIsMotionDummy);
+
+struct ParamsProcessorIsMotionDummy : public ParamsProcessorBase, public ParamsIsMotion
+{
+        std::string state_structure = "PO";
+
+        ParamsProcessorIsMotionDummy() = default;
+        ParamsProcessorIsMotionDummy(std::string _unique_name, const ParamsServer& _server):
+            ParamsProcessorBase(_unique_name, _server),
+            ParamsIsMotion(_unique_name, _server)
+        {
+
+        };
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorIsMotionDummy);
+
+class ProcessorIsMotionDummy : public ProcessorBase, public IsMotion
+{
+    public:
+        ProcessorIsMotionDummy(ParamsProcessorIsMotionDummyPtr _params) :
+            ProcessorBase("ProcessorIsMotionDummy", 2, _params),
+            IsMotion(_params->state_structure, _params)
+        {}
+        ~ProcessorIsMotionDummy(){};
+
+        // Factory method for high level API
+        WOLF_PROCESSOR_CREATE(ProcessorIsMotionDummy, ParamsProcessorIsMotionDummy);
+
+        void configure(SensorBasePtr _sensor) override {};
+        void processCapture(CaptureBasePtr) override {};
+        void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {};
+        bool triggerInCapture(CaptureBasePtr) const override { return false; };
+        bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) 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 StateStructure& _structure = "") const override
+        {
+            return getOdometry();
+        };
+
+        VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override
+        {
+            return getOdometry();
+        };
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorIsMotionDummy);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorIsMotionDummy);
+} // namespace wolf
+
+#endif /* TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_ */

test/gtest_is_motion.cpp

+//Wolf
+#include "core/utils/utils_gtest.h"
+
+#include "core/processor/is_motion.h"
+#include "dummy/processor_is_motion_dummy.h"
+#include "core/sensor/sensor_odom_2d.h"
+
+#include "core/problem/problem.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+
+class IsMotionTest : public testing::Test
+{
+    public:
+        ProblemPtr problem;
+        SensorBasePtr sen;
+        ProcessorBasePtr prc1, prc2, prc3;
+        IsMotionPtr im1, im2, im3;
+
+        std::string wolf_root = _WOLF_ROOT_DIR;
+        double dt = 0.01;
+
+        void SetUp() override
+        {
+            // Wolf problem
+            problem = Problem::create("POV", 2);
+
+            // Install odom sensor
+            sen = problem->installSensor("SensorOdom2d",
+                                         "odometer",
+                                         Vector3d(0,0,0),
+                                         wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+            // Install 3 odom processors
+            ParamsProcessorIsMotionDummyPtr prc1_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+            prc1_params->time_tolerance = dt/2;
+            prc1_params->state_structure = "PO";
+            prc1_params->state_getter = false;
+            prc1 = problem->installProcessor("ProcessorIsMotionDummy",
+                                             "not getter processor",
+                                             sen,
+                                             prc1_params);
+            im1 = std::dynamic_pointer_cast<IsMotion>(prc1);
+
+            ParamsProcessorIsMotionDummyPtr prc2_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+            prc2_params->time_tolerance = dt/2;
+            prc2_params->state_structure = "PO";
+            prc2_params->state_getter = true;
+            prc2_params->state_priority = 1;
+            prc2 = problem->installProcessor("ProcessorIsMotionDummy",
+                                             "getter processor",
+                                             sen,
+                                             prc2_params);
+            im2 = std::dynamic_pointer_cast<IsMotion>(prc2);
+
+            ParamsProcessorIsMotionDummyPtr prc3_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+            prc3_params->time_tolerance = dt/2;
+            prc3_params->state_structure = "POV";
+            prc3_params->state_getter = true;
+            prc3_params->state_priority = 1;
+            prc3 = problem->installProcessor("ProcessorIsMotionDummy",
+                                             "getter processor low priority",
+                                             sen,
+                                             prc3_params);
+            im3 = std::dynamic_pointer_cast<IsMotion>(prc3);
+        }
+};
+
+/*
+ * Things to be tested:
+ * - state_getter
+ * - state_priority
+ * - Duplicated priority (Problem handles this)
+ * - setOdometry (stateStructures)
+ * - getOdomtry
+ * - Problem::getState/getOdometry (state_getter, state_priority)
+ */
+
+TEST_F(IsMotionTest, install)
+{
+    // All isMotion() = true
+    ASSERT_TRUE (prc1->isMotion());
+    ASSERT_TRUE (prc2->isMotion());
+    ASSERT_TRUE (prc3->isMotion());
+    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->getStatePriority(), 1);
+    ASSERT_EQ(im3->getStatePriority(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
+    ASSERT_EQ(im1->getStateStructure(), "PO");
+    ASSERT_EQ(im2->getStateStructure(), "PO");
+    ASSERT_EQ(im3->getStateStructure(), "POV");
+
+    // Only 2 and 3 in problem::processor_is_motion_map_
+    ASSERT_EQ(problem->getProcessorIsMotionMap().size(), 2);
+    ASSERT_EQ(problem->getProcessorIsMotionMap().begin()->second, im2);
+    ASSERT_EQ(std::next(problem->getProcessorIsMotionMap().begin())->second, im3);
+}
+
+TEST_F(IsMotionTest, odometry)
+{
+    VectorComposite odom_p("P"); // missing P key
+    odom_p['P'] = Vector2d::Zero();
+    VectorComposite odom_pov("POV"); // key V not needed by prc1 and prc2
+    odom_pov['P'] = Vector2d::Zero();
+    odom_pov['O'] = Vector1d::Zero();
+    odom_pov['V'] = Vector2d::Zero();
+
+    // Error: required PO keys to be added
+    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");
+    odom1['P'] = Vector2d::Zero();
+    odom1['O'] = Vector1d::Zero();
+    im1->setOdometry(odom1);
+    auto odom1_get = im1->getOdometry();
+    EXPECT_TRUE(odom1_get.count('P') == 1);
+    EXPECT_TRUE(odom1_get.count('O') == 1);
+    EXPECT_MATRIX_APPROX(odom1_get.at('P'), odom1.at('P'), 1e-9);
+    EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9);
+
+    // im1 ->set odom = 1, 1, 1
+    VectorComposite odom2("PO");
+    odom2['P'] = Vector2d::Ones();
+    odom2['O'] = Vector1d::Ones();
+    im2->setOdometry(odom2);
+    auto odom2_get = im2->getOdometry();
+    EXPECT_TRUE(odom2_get.count('P') == 1);
+    EXPECT_TRUE(odom2_get.count('O') == 1);
+    EXPECT_MATRIX_APPROX(odom2_get.at('P'), odom2.at('P'), 1e-9);
+    EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9);
+
+    // im1 ->set odom = 2, 2, 2, 2, 2
+    VectorComposite odom3("POV");
+    odom3['P'] = 2 * Vector2d::Ones();
+    odom3['O'] = 2 * Vector1d::Ones();
+    odom3['V'] = 2 * Vector2d::Ones();
+    im3->setOdometry(odom3);
+    auto odom3_get = im3->getOdometry();
+    EXPECT_TRUE(odom3_get.count('P') == 1);
+    EXPECT_TRUE(odom3_get.count('O') == 1);
+    EXPECT_TRUE(odom3_get.count('V') == 1);
+    EXPECT_MATRIX_APPROX(odom3_get.at('P'), odom3.at('P'), 1e-9);
+    EXPECT_MATRIX_APPROX(odom3_get.at('O'), odom3.at('O'), 1e-9);
+    EXPECT_MATRIX_APPROX(odom3_get.at('V'), odom3.at('V'), 1e-9);
+
+    // problem::getOdometry(): by priority P and O should come from im2 and V from im3
+    auto odom_get = problem->getOdometry();
+    EXPECT_TRUE(odom_get.count('P') == 1);
+    EXPECT_TRUE(odom_get.count('O') == 1);
+    EXPECT_TRUE(odom_get.count('V') == 1);
+    EXPECT_MATRIX_APPROX(odom_get.at('P'), odom2.at('P'), 1e-9);
+    EXPECT_MATRIX_APPROX(odom_get.at('O'), odom2.at('O'), 1e-9);
+    EXPECT_MATRIX_APPROX(odom_get.at('V'), odom3.at('V'), 1e-9);
+
+    // problem::getState(): by priority P and O should come from im2 and V from im3
+    auto state_get = problem->getState();
+    EXPECT_TRUE(state_get.count('P') == 1);
+    EXPECT_TRUE(state_get.count('O') == 1);
+    EXPECT_TRUE(state_get.count('V') == 1);
+    EXPECT_MATRIX_APPROX(state_get.at('P'), odom2.at('P'), 1e-9);
+    EXPECT_MATRIX_APPROX(state_get.at('O'), odom2.at('O'), 1e-9);
+    EXPECT_MATRIX_APPROX(state_get.at('V'), odom3.at('V'), 1e-9);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/gtest_processor_base.cpp

@@ -48,10 +48,16 @@ TEST(ProcessorBase, IsMotion)
     auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy",  "dummy", sens_trk);
 
     // Install odometer (sensor and processor)
-    SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+    SensorBasePtr sens_odo = problem->installSensor("SensorOdom2d",
+                                                    "odometer",
+                                                    Vector3d(0,0,0),
+                                                    wolf_root + "/test/yaml/sensor_odom_2d.yaml");
     ParamsProcessorOdom2dPtr proc_odo_params = make_shared<ParamsProcessorOdom2d>();
     proc_odo_params->time_tolerance = dt/2;
-    ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d", "odom processor", sens_odo, proc_odo_params);
+    ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom2d",
+                                                          "odom processor",
+                                                          sens_odo,
+                                                          proc_odo_params);
 
     ASSERT_FALSE(proc_trk->isMotion());
     ASSERT_TRUE (proc_odo->isMotion());