fast_slam

1#include <gtest/gtest.h>
2
3#include "fast_slam/PathList.hpp"
4#include <eigen3/Eigen/Core>
5#include <chrono>
6
7class PathTester : public ::testing::Test {
8protected:
9    Eigen::Vector3d initial_pose = {1.0 , 2.0 , 3.0};
10    std::unique_ptr<fastslam::Path> path = std::make_unique<fastslam::Path>(initial_pose, 1);
11
12    virtual void SetUp(){
13        for(size_t i=2;i<=10; i++){
14            std::chrono::nanoseconds time = std::chrono::nanoseconds(10);
15            Eigen::Vector3d pose;
16            pose << 1.0 * i, 2.0 * i, 3.0 * i;
17
18            path->addPose(pose, i, time);
19        }
20    }
21};
22
23TEST_F(PathTester, pathLength_test){
24    auto length = path->getLength();
25    EXPECT_EQ(length, 10);
26}
27
28TEST_F(PathTester, latestPose_test){
29    auto pose = path->getPose();
30    EXPECT_EQ(pose(0), 1.0 * 10);
31    EXPECT_EQ(pose(1), 2.0 * 10);
32    EXPECT_EQ(pose(2), 3.0 * 10);
33}
34
35TEST_F(PathTester, specificPose_test){
36    auto pose = path->getPose(5);
37    EXPECT_EQ(pose(0), 1.0 * 5);
38    EXPECT_EQ(pose(1), 2.0 * 5);
39    EXPECT_EQ(pose(2), 3.0 * 5);
40}
41
42TEST_F(PathTester, copy_test){
43    std::unique_ptr<fastslam::Path> path_copy = std::make_unique<fastslam::Path>(*path);
44
45    std::stringstream out, out_copy;
46    out << *path;
47    out_copy << *path_copy;
48
49    EXPECT_EQ(out.str(), out_copy.str());
50}
51
52TEST_F(PathTester, copyAndAdd_test){
53    std::unique_ptr<fastslam::Path> path_copy = std::make_unique<fastslam::Path>(*path);
54
55    std::chrono::nanoseconds time = std::chrono::nanoseconds(10);
56    Eigen::Vector3d pose;
57    uint32_t i = 11;
58
59    // add pose to original path
60    pose << 1.0 * i, 2.0 * i, 3.0 * i;
61    path->addPose(pose, i, time);
62
63    // add same pose to copied path
64    path_copy->addPose(pose, i, time);
65
66
67    std::stringstream out, out_copy;
68    out << *path;
69    out_copy << *path_copy;
70
71
72    for(size_t i=1; i<=11; i++){
73        std::string line, line_copy;
74        std::getline(out, line);
75        std::getline(out_copy, line_copy);
76        if(i == 11){
77            EXPECT_FALSE(line == line_copy);
78        }
79        else{
80            EXPECT_TRUE(line == line_copy);
81        }
82    }
83}