1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
|
/*
* Copyright (C) 2019 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <gtest/gtest.h>
#include <thread>
#include "ignition/math/Angle.hh"
#include "ignition/math/Helpers.hh"
#include "ignition/math/DiffDriveOdometry.hh"
using namespace ignition;
/////////////////////////////////////////////////
TEST(DiffDriveOdometryTest, DiffDriveOdometry)
{
math::DiffDriveOdometry odom;
EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
EXPECT_DOUBLE_EQ(0.0, odom.X());
EXPECT_DOUBLE_EQ(0.0, odom.Y());
EXPECT_DOUBLE_EQ(0.0, odom.LinearVelocity());
EXPECT_DOUBLE_EQ(0.0, *odom.AngularVelocity());
double wheelSeparation = 2.0;
double wheelRadius = 0.5;
double wheelCircumference = 2 * IGN_PI * wheelRadius;
// This is the linear distance traveled per degree of wheel rotation.
double distPerDegree = wheelCircumference / 360.0;
// Setup the wheel parameters, and initialize
odom.SetWheelParams(wheelSeparation, wheelRadius, wheelRadius);
auto startTime = std::chrono::steady_clock::now();
odom.Init(startTime);
// Sleep for a little while, then update the odometry with the new wheel
// position.
auto time1 = startTime + std::chrono::milliseconds(100);
odom.Update(IGN_DTOR(1.0), IGN_DTOR(1.0), time1);
EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
EXPECT_DOUBLE_EQ(distPerDegree, odom.X());
EXPECT_DOUBLE_EQ(0.0, odom.Y());
// Linear velocity should be dist_traveled / time_elapsed.
EXPECT_NEAR(distPerDegree / 0.1, odom.LinearVelocity(), 1e-3);
// Angular velocity should be zero since the "robot" is traveling in a
// straight line.
EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3);
// Sleep again, then update the odometry with the new wheel position.
auto time2 = time1 + std::chrono::milliseconds(100);
odom.Update(IGN_DTOR(2.0), IGN_DTOR(2.0), time2);
EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
EXPECT_NEAR(distPerDegree * 2.0, odom.X(), 3e-6);
EXPECT_DOUBLE_EQ(0.0, odom.Y());
// Linear velocity should be dist_traveled / time_elapsed.
EXPECT_NEAR(distPerDegree / 0.1, odom.LinearVelocity(), 1e-3);
// Angular velocity should be zero since the "robot" is traveling in a
// straight line.
EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3);
// Initialize again, and odom values should be reset.
startTime = std::chrono::steady_clock::now();
odom.Init(startTime);
EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
EXPECT_DOUBLE_EQ(0.0, odom.X());
EXPECT_DOUBLE_EQ(0.0, odom.Y());
EXPECT_DOUBLE_EQ(0.0, odom.LinearVelocity());
EXPECT_DOUBLE_EQ(0.0, *odom.AngularVelocity());
// Sleep again, this time move 2 degrees in 100ms.
time1 = startTime + std::chrono::milliseconds(100);
odom.Update(IGN_DTOR(2.0), IGN_DTOR(2.0), time1);
EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
EXPECT_NEAR(distPerDegree * 2.0, odom.X(), 3e-6);
EXPECT_DOUBLE_EQ(0.0, odom.Y());
// Linear velocity should be dist_traveled / time_elapsed.
EXPECT_NEAR(distPerDegree * 2 / 0.1, odom.LinearVelocity(), 1e-3);
// Angular velocity should be zero since the "robot" is traveling in a
// straight line.
EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3);
// Sleep again, this time rotate the right wheel by 1 degree.
time2 = time1 + std::chrono::milliseconds(100);
odom.Update(IGN_DTOR(2.0), IGN_DTOR(3.0), time2);
// The heading should be the arc tangent of the linear distance traveled
// by the right wheel (the left wheel was stationary) divided by the
// wheel separation.
EXPECT_NEAR(atan2(distPerDegree, wheelSeparation), *odom.Heading(), 1e-6);
// The X odom reading should have increased by the sine of the heading *
// half the wheel separation.
double xDistTraveled =
sin(atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5;
double prevXPos = distPerDegree * 2.0;
EXPECT_NEAR(xDistTraveled + prevXPos, odom.X(), 3e-6);
// The Y odom reading should have increased by the cosine of the header *
// half the wheel separation.
double yDistTraveled = (wheelSeparation * 0.5) -
cos(atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5;
double prevYPos = 0.0;
EXPECT_NEAR(yDistTraveled + prevYPos, odom.Y(), 3e-6);
// Angular velocity should be the difference between the x and y distance
// traveled divided by the wheel separation divided by the seconds
// elapsed.
EXPECT_NEAR(
((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1,
*odom.AngularVelocity(), 1e-3);
}
|
