From a460682e0ca1f0d1ea55aa287b270b1ab19f56b7 Mon Sep 17 00:00:00 2001 From: Franz Rammerstorfer Date: Mon, 11 Dec 2023 16:42:24 +0100 Subject: [PATCH] add odometry unit tests --- steering_controllers_library/CMakeLists.txt | 3 + .../test/test_steering_odometry.cpp | 122 ++++++++++++++++++ 2 files changed, 125 insertions(+) create mode 100644 steering_controllers_library/test/test_steering_odometry.cpp diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 4a98dbf320..6b99416d9f 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -67,6 +67,9 @@ if(BUILD_TESTING) controller_interface hardware_interface ) + ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) + target_link_libraries(test_steering_odometry steering_controllers_library) + endif() install( diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp new file mode 100644 index 0000000000..778eaedc15 --- /dev/null +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -0,0 +1,122 @@ +// +// 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 "gmock/gmock.h" + +#include "steering_controllers_library/steering_odometry.hpp" + +namespace +{ + const double EPS = 1e-8; +} // unnamed namespace + +using namespace steering_odometry; + +TEST(TestSteeringOdometry, initialize) +{ + EXPECT_NO_THROW(SteeringOdometry()); + + SteeringOdometry odom(1); + odom.set_wheel_params(1.,2.,3.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + EXPECT_FLOAT_EQ(odom.get_heading(), 0.); + EXPECT_FLOAT_EQ(odom.get_x(), 0.); + EXPECT_FLOAT_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +{ + SteeringOdometry odom(1); + odom.set_wheel_params(1.,2.,1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(2., 0., 0.5); + EXPECT_FLOAT_EQ(odom.get_linear(), 2.); + EXPECT_FLOAT_EQ(odom.get_x(), 1.); + EXPECT_FLOAT_EQ(odom.get_y(), 0.); +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +{ + SteeringOdometry odom(1); + odom.set_wheel_params(1.,2.,1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 1., 1.); + EXPECT_FLOAT_EQ(odom.get_linear(), 1.); + EXPECT_FLOAT_EQ(odom.get_angular(), 1.); + + EXPECT_TRUE(odom.get_x() > 0); // pos x + EXPECT_TRUE(odom.get_y() > 0); // pos y, ie. left + +} + +TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +{ + SteeringOdometry odom(1); + odom.set_wheel_params(1.,2.,1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., -1., 1.); + EXPECT_FLOAT_EQ(odom.get_linear(), 1.); + EXPECT_FLOAT_EQ(odom.get_angular(), -1.); + + EXPECT_TRUE(odom.get_x() > 0); // pos x + EXPECT_TRUE(odom.get_y() < 0); // neg y ie. right +} + +TEST(TestSteeringOdometry, ackermann_back_kin_linear) +{ + SteeringOdometry odom(1); + odom.set_wheel_params(1.,2.,1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0.); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_TRUE(cmd0[0] == cmd0[1]); // linear + EXPECT_TRUE(cmd0[0] > 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_TRUE(cmd1[0] == cmd1[1]); // no steering + EXPECT_TRUE(cmd1[0] == 0); +} + +TEST(TestSteeringOdometry, ackermann_back_kin_left) +{ + SteeringOdometry odom(1); + odom.set_wheel_params(1.,2.,1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0.,0.2,1.); // assume already turn + auto cmd = odom.get_commands(1.,0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_TRUE(cmd0[0] > cmd0[1]); // right (outer) > left (inner) + EXPECT_TRUE(cmd0[0] > 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_TRUE(cmd1[0] < cmd1[1]); // right (outer) < left (inner) + EXPECT_TRUE(cmd1[0] > 0); + // std::cout << "\nsteer right (outer)" << cmd1[0] << std::endl; + // std::cout << "\nsteer left (inner)" << cmd1[1] << std::endl; +} + +TEST(TestSteeringOdometry, ackermann_back_kin_right) +{ + SteeringOdometry odom(1); + odom.set_wheel_params(1.,2.,1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + odom.update_from_position(0.,-0.2,1.); // assume already turn + auto cmd = odom.get_commands(1.,-0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_TRUE(cmd0[0] < cmd0[1]); // right (inner) < left outer) + EXPECT_TRUE(cmd0[0] > 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_TRUE(std::abs(cmd1[0]) > std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_TRUE(cmd1[0] < 0); + //std::cout << "\nsteer right (inner)" << cmd1[0] << std::endl; + //std::cout << "\nsteer left (outer)" << cmd1[1] << std::endl; +} \ No newline at end of file