Fix ackermann steering odometry (#921) (#956)

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(

steering_controllers_library/src/steering_odometry.cpp

@@ -270,8 +270,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);

steering_controllers_library/test/test_steering_odometry.cpp

@@ -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);
+}