Skip to content

Commit

Permalink
Fix ackermann steering odometry (#921) (#955)
Browse files Browse the repository at this point in the history
  • Loading branch information
mergify[bot] authored Jan 3, 2024
1 parent 5fdee99 commit cb10e3e
Show file tree
Hide file tree
Showing 3 changed files with 115 additions and 2 deletions.
3 changes: 3 additions & 0 deletions steering_controllers_library/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
4 changes: 2 additions & 2 deletions steering_controllers_library/src/steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -272,8 +272,8 @@ std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_comma
double denominator_first_member = 2 * wheelbase_ * std::cos(alpha);
double denominator_second_member = wheel_track_ * std::sin(alpha);

double alpha_r = std::atan2(numerator, denominator_first_member - denominator_second_member);
double alpha_l = std::atan2(numerator, denominator_first_member + denominator_second_member);
double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member);
double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member);
steering_commands = {alpha_r, alpha_l};
}
return std::make_tuple(traction_commands, steering_commands);
Expand Down
110 changes: 110 additions & 0 deletions steering_controllers_library/test/test_steering_odometry.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
// Copyright (c) 2023, Virtual Vehicle Research GmbH
//
// 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"

TEST(TestSteeringOdometry, initialize)
{
EXPECT_NO_THROW(steering_odometry::SteeringOdometry());

steering_odometry::SteeringOdometry odom(1);
odom.set_wheel_params(1., 2., 3.);
odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
EXPECT_DOUBLE_EQ(odom.get_heading(), 0.);
EXPECT_DOUBLE_EQ(odom.get_x(), 0.);
EXPECT_DOUBLE_EQ(odom.get_y(), 0.);
}

TEST(TestSteeringOdometry, ackermann_fwd_kin_linear)
{
steering_odometry::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_DOUBLE_EQ(odom.get_linear(), 2.);
EXPECT_DOUBLE_EQ(odom.get_x(), 1.);
EXPECT_DOUBLE_EQ(odom.get_y(), 0.);
}

TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left)
{
steering_odometry::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_DOUBLE_EQ(odom.get_linear(), 1.);
EXPECT_DOUBLE_EQ(odom.get_angular(), 1.);

EXPECT_GT(odom.get_x(), 0); // pos x
EXPECT_GT(odom.get_y(), 0); // pos y, ie. left
}

TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right)
{
steering_odometry::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_DOUBLE_EQ(odom.get_linear(), 1.);
EXPECT_DOUBLE_EQ(odom.get_angular(), -1.);
EXPECT_GT(odom.get_x(), 0); // pos x
EXPECT_LT(odom.get_y(), 0); // neg y ie. right
}

TEST(TestSteeringOdometry, ackermann_back_kin_linear)
{
steering_odometry::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_EQ(cmd0[0], cmd0[1]); // linear
EXPECT_GT(cmd0[0], 0);
auto cmd1 = std::get<1>(cmd); // steer
EXPECT_EQ(cmd1[0], cmd1[1]); // no steering
EXPECT_EQ(cmd1[0], 0);
}

TEST(TestSteeringOdometry, ackermann_back_kin_left)
{
steering_odometry::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_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner)
EXPECT_GT(cmd0[0], 0);
auto cmd1 = std::get<1>(cmd); // steer
EXPECT_LT(cmd1[0], cmd1[1]); // right (outer) < left (inner)
EXPECT_GT(cmd1[0], 0);
}

TEST(TestSteeringOdometry, ackermann_back_kin_right)
{
steering_odometry::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_LT(cmd0[0], cmd0[1]); // right (inner) < left outer)
EXPECT_GT(cmd0[0], 0);
auto cmd1 = std::get<1>(cmd); // steer
EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer)
EXPECT_LT(cmd1[0], 0);
}

0 comments on commit cb10e3e

Please sign in to comment.