From 03e511c5319f7f14cb8f0317d612c40b159dde85 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 01/13] Fix IK - in open-loop mode, zero steering angle was used for IK calculation - wrong wheelspeed was used (traction on steering wheel, instead of other axle was assumed) --- .../steering_odometry.hpp | 3 +- .../src/steering_controllers_library.cpp | 2 +- .../src/steering_odometry.cpp | 40 ++++++++++++++----- .../test/test_steering_odometry.cpp | 6 +-- 4 files changed, 35 insertions(+), 16 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 882d97f5dc..02bdb4cdbe 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -183,10 +183,11 @@ class SteeringOdometry * \brief Calculates inverse kinematics for the desired linear and angular velocities * \param v_bx Desired linear velocity of the robot in x_b-axis direction * \param omega_bz Desired angular velocity of the robot around x_z-axis + * \param open_loop If false, the IK will be calculated using measured steering angle * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz); + const double v_bx, const double omega_bz, const bool open_loop); /** * \brief Reset poses, heading, and accumulators diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 167ee6aecf..cf85ac3e41 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -450,7 +450,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_angular_velocity_ = reference_interfaces_[1]; auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); + odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index feefbc89bc..8666471644 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -21,6 +21,7 @@ #include #include +#include namespace steering_odometry { @@ -181,30 +182,42 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (omega_bz == 0 || v_bx == 0) + if (fabs(v_bx) < std::numeric_limits::epsilon()) { - return 0; + // avoid division by zero + return 0.; } return std::atan(omega_bz * wheelbase_ / v_bx); } std::tuple, std::vector> SteeringOdometry::get_commands( - const double v_bx, const double omega_bz) + const double v_bx, const double omega_bz, const bool open_loop) { // desired wheel speed and steering angle of the middle of traction and steering axis - double Ws, phi; + double Ws, phi, phi_IK = steer_pos_; +#if 0 if (v_bx == 0 && omega_bz != 0) { - // TODO(anyone) would be only valid if traction is on the steering axis -> tricycle_controller + // TODO(anyone) this would be only possible if traction is on the steering axis phi = omega_bz > 0 ? M_PI_2 : -M_PI_2; Ws = abs(omega_bz) * wheelbase_ / wheel_radius_; } else { - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); - Ws = v_bx / (wheel_radius_ * std::cos(steer_pos_)); + // TODO(anyone) this would be valid only if traction is on the steering axis + Ws = v_bx / (wheel_radius_ * std::cos(phi_IK)); // using the measured steering angle } +#endif + // steering angle + phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + if (open_loop) + { + phi_IK = phi; + } + // wheel speed + Ws = v_bx / wheel_radius_; + std::cout << "Ws: " << Ws << " phi: " << phi << " phi_IK: " << phi_IK << std::endl; if (config_type_ == BICYCLE_CONFIG) { @@ -216,17 +229,20 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + // double-traction axle + if (fabs(phi_IK) < 1e-6) { + // avoid division by zero traction_commands = {Ws, Ws}; } else { - const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double turning_radius = wheelbase_ / std::tan(phi_IK); const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } + // simple steering steering_commands = {phi}; return std::make_tuple(traction_commands, steering_commands); } @@ -234,14 +250,16 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(steer_pos_) < 1e-6) + if (fabs(phi_IK) < 1e-6) { + // avoid division by zero traction_commands = {Ws, Ws}; + // shortcut, no steering steering_commands = {phi, phi}; } else { - const double turning_radius = wheelbase_ / std::tan(steer_pos_); + const double turning_radius = wheelbase_ / std::tan(phi_IK); const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3e4adc59fe..b9bd2afc3d 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -70,7 +70,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_linear) 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 cmd = odom.get_commands(1., 0., true); auto cmd0 = std::get<0>(cmd); // vel EXPECT_EQ(cmd0[0], cmd0[1]); // linear EXPECT_GT(cmd0[0], 0); @@ -85,7 +85,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_left) 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 cmd = odom.get_commands(1., 0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_GT(cmd0[0], cmd0[1]); // right (outer) > left (inner) EXPECT_GT(cmd0[0], 0); @@ -100,7 +100,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) 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 cmd = odom.get_commands(1., -0.1, false); auto cmd0 = std::get<0>(cmd); // vel EXPECT_LT(cmd0[0], cmd0[1]); // right (inner) < left outer) EXPECT_GT(cmd0[0], 0); From f0fc6df676c93bd3ae2086348f3f42c4b32ad088 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 02/13] Fix controller tests and add notes --- .../test/test_ackermann_steering_controller.cpp | 6 +++++- .../test/test_bicycle_steering_controller.cpp | 10 +++++----- .../test/test_tricycle_steering_controller.cpp | 3 +++ 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index ef5454a16c..e4f0aae146 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -173,6 +173,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -211,8 +212,9 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR( + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands + EXPECT_NEAR( controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( @@ -261,6 +263,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -276,6 +279,7 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[CMD_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 3dcdc0b1db..db7a9da67e 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -158,7 +158,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -190,7 +190,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); @@ -237,7 +237,7 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status EXPECT_EQ(msg.linear_velocity_command[0], 1.1); EXPECT_EQ(msg.steering_angle_command[0], 2.2); - publish_commands(); + publish_commands(0.1, 0.2); ASSERT_TRUE(controller_->wait_for_commands(executor)); ASSERT_EQ( @@ -245,14 +245,14 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.253221, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR( controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); - EXPECT_NEAR(msg.linear_velocity_command[0], 0.253221, COMMON_THRESHOLD); + EXPECT_NEAR(msg.linear_velocity_command[0], 0.1 / 0.45, COMMON_THRESHOLD); EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index c555de53de..328f5e4d6a 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -201,6 +201,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -246,6 +247,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, COMMON_THRESHOLD); @@ -258,6 +260,7 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu subscribe_and_get_messages(msg); + // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( msg.linear_velocity_command[STATE_TRACTION_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( From 6ac732ccc8f78ba4305bb9f45f50c7c85bd8f9ec Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 03/13] Improve tests for other steering types --- .../test/test_steering_odometry.cpp | 119 +++++++++++++++--- 1 file changed, 103 insertions(+), 16 deletions(-) diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index b9bd2afc3d..d39130c02f 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -28,7 +28,21 @@ TEST(TestSteeringOdometry, initialize) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) +// ----------------- Ackermann ----------------- + +TEST(TestSteeringOdometry, ackermann_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + +TEST(TestSteeringOdometry, ackermann_odometry_openloop_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -39,7 +53,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_linear) EXPECT_DOUBLE_EQ(odom.get_y(), 0.); } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -52,7 +66,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_left) EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } -TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -64,7 +78,7 @@ TEST(TestSteeringOdometry, ackermann_fwd_kin_angular_right) EXPECT_LT(odom.get_y(), 0); // neg y ie. right } -TEST(TestSteeringOdometry, ackermann_back_kin_linear) +TEST(TestSteeringOdometry, ackermann_IK_linear) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -79,7 +93,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_linear) EXPECT_EQ(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_left) +TEST(TestSteeringOdometry, ackermann_IK_left) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -94,7 +108,7 @@ TEST(TestSteeringOdometry, ackermann_back_kin_left) EXPECT_GT(cmd1[0], 0); } -TEST(TestSteeringOdometry, ackermann_back_kin_right) +TEST(TestSteeringOdometry, ackermann_IK_right) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 2., 1.); @@ -109,6 +123,47 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_LT(cmd1[0], 0); } +// ----------------- bicycle ----------------- + +TEST(TestSteeringOdometry, bicycle_IK_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_DOUBLE_EQ(cmd1[0], 0); // no steering +} + +TEST(TestSteeringOdometry, bicycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, bicycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::BICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_DOUBLE_EQ(cmd0[0], 1.0); // equals linear + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], 0); // left steering +} + TEST(TestSteeringOdometry, bicycle_odometry) { steering_odometry::SteeringOdometry odom(1); @@ -121,24 +176,56 @@ TEST(TestSteeringOdometry, bicycle_odometry) EXPECT_NEAR(odom.get_heading(), .01, 1e-3); } -TEST(TestSteeringOdometry, tricycle_odometry) +// ----------------- tricycle ----------------- + +TEST(TestSteeringOdometry, tricycle_IK_linear) { steering_odometry::SteeringOdometry odom(1); - odom.set_wheel_params(1., 1., 1.); + odom.set_wheel_params(1., 2., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); - EXPECT_NEAR(odom.get_angular(), .1, 1e-3); - EXPECT_NEAR(odom.get_x(), .1, 1e-3); - EXPECT_NEAR(odom.get_heading(), .01, 1e-3); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0., true); + 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], 0); // no steering } -TEST(TestSteeringOdometry, ackermann_odometry) +TEST(TestSteeringOdometry, tricycle_IK_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1, false); + 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_GT(cmd1[0], 0); // left steering +} + +TEST(TestSteeringOdometry, tricycle_IK_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1, false); + 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_LT(cmd1[0], 0); // right steering +} + +TEST(TestSteeringOdometry, tricycle_odometry) { steering_odometry::SteeringOdometry odom(1); odom.set_wheel_params(1., 1., 1.); - odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); - ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); EXPECT_NEAR(odom.get_x(), .1, 1e-3); From 9fc783f18e7b4414cf79b921fe1efbaaeec2420c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 04/13] Improve odometry of overdetermined measurements --- .../steering_odometry.hpp | 10 ++++++ .../src/steering_odometry.cpp | 31 ++++++++++++++++--- .../test/test_steering_odometry.cpp | 4 +-- 3 files changed, 38 insertions(+), 7 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 02bdb4cdbe..2bea1d47f8 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -226,6 +226,16 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates linear velocity of a robot with double traction axle + * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] + * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] + * \param steer_pos Steer wheel position [rad] + */ + double get_lin_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos); + /** * \brief Reset linear and angular accumulators */ diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 8666471644..5321ce74a5 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -129,13 +129,26 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } +double SteeringOdometry::get_lin_velocity_double_traction_axle( + const double right_traction_wheel_vel, const double left_traction_wheel_vel, + const double steer_pos) +{ + double turning_radius = wheelbase_ / std::tan(steer_pos); + // overdetermined, we take the average + double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius / + (turning_radius + wheel_track_ * 0.5); + double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius / + (turning_radius - wheel_track_ * 0.5); + return (vel_r + vel_l) * 0.5; +} + bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos, const double dt) { - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; steer_pos_ = steer_pos; + double linear_velocity = get_lin_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; @@ -146,9 +159,17 @@ bool SteeringOdometry::update_from_velocity( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double right_steer_pos, const double left_steer_pos, const double dt) { - steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5; - double linear_velocity = - (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5; + // overdetermined, we take the average + const double right_steer_pos_est = std::atan( + wheelbase_ * std::tan(right_steer_pos) / + (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos))); + const double left_steer_pos_est = std::atan( + wheelbase_ * std::tan(left_steer_pos) / + (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); + steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; + + double linear_velocity = get_lin_velocity_double_traction_axle( + right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index d39130c02f..d93e29eca5 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -36,7 +36,7 @@ TEST(TestSteeringOdometry, ackermann_odometry) odom.set_wheel_params(1., 1., 1.); odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); @@ -226,7 +226,7 @@ TEST(TestSteeringOdometry, tricycle_odometry) odom.set_wheel_params(1., 1., 1.); odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1)); - EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3); EXPECT_NEAR(odom.get_angular(), .1, 1e-3); EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); From aa9126039d68e54eb666a7fc280933378424c105 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 May 2024 19:54:14 +0000 Subject: [PATCH 05/13] Remove debug output --- steering_controllers_library/src/steering_odometry.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 5321ce74a5..fa22dab2ed 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -238,7 +238,6 @@ std::tuple, std::vector> SteeringOdometry::get_comma } // wheel speed Ws = v_bx / wheel_radius_; - std::cout << "Ws: " << Ws << " phi: " << phi << " phi_IK: " << phi_IK << std::endl; if (config_type_ == BICYCLE_CONFIG) { From 91d67e8d678ffdf47ef5827bc8c4f692480d646b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 8 Jun 2024 15:03:05 +0200 Subject: [PATCH 06/13] Use std:: namespace for trigonometric functions --- .../src/steering_odometry.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index fa22dab2ed..97ef60a708 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -124,7 +124,7 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -150,7 +150,7 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_lin_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -317,8 +317,8 @@ void SteeringOdometry::integrate_runge_kutta_2( const double theta_mid = heading_ + omega_bz * 0.5 * dt; // Use the intermediate values to update the state - x_ += v_bx * cos(theta_mid) * dt; - y_ += v_bx * sin(theta_mid) * dt; + x_ += v_bx * std::cos(theta_mid) * dt; + y_ += v_bx * std::sin(theta_mid) * dt; heading_ += omega_bz * dt; } @@ -338,8 +338,8 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double heading_old = heading_; const double R = delta_x_b / delta_theta; heading_ += delta_theta; - x_ += R * (sin(heading_) - sin(heading_old)); - y_ += -R * (cos(heading_) - cos(heading_old)); + x_ += R * (sin(heading_) - std::sin(heading_old)); + y_ += -R * (cos(heading_) - std::cos(heading_old)); } } From d81c85b4c68ca6db18d8223639d250b21faa8fd3 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Tue, 11 Jun 2024 14:57:15 +0000 Subject: [PATCH 07/13] Included option to use Ackermann control via steering angle and linear velocity --- .../src/ackermann_steering_controller.cpp | 2 +- .../src/bicycle_steering_controller.cpp | 2 +- .../steering_controllers_library.hpp | 6 +- .../steering_odometry.hpp | 12 +- .../src/steering_controllers_library.cpp | 140 +++++++++++++++--- .../src/steering_controllers_library.yaml | 7 + .../src/steering_odometry.cpp | 16 +- .../src/tricycle_steering_controller.cpp | 2 +- 8 files changed, 153 insertions(+), 34 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d9d95bf8b5..af27b170e8 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -58,7 +58,7 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio { if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + odometry_.update_open_loop(last_linear_velocity_, last_angular_command_, period.seconds(), params_.twist_input); } else { diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 95eaf1965c..6bd866e6ef 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -56,7 +56,7 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) { if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + odometry_.update_open_loop(last_linear_velocity_, last_angular_command_, period.seconds(), params_.twist_input); } else { diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index b560e2a782..d0f00dd51d 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -97,9 +97,10 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; - rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; + rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; + realtime_tools::RealtimeBuffer> input_ackermann_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; @@ -134,7 +135,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // store last velocity double last_linear_velocity_ = 0.0; - double last_angular_velocity_ = 0.0; + double last_angular_command_ = 0.0; std::vector rear_wheels_state_names_; std::vector front_wheels_state_names_; @@ -144,6 +145,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( const std::shared_ptr msg); void reference_callback_unstamped(const std::shared_ptr msg); + void reference_callback_ackermann(const std::shared_ptr msg); }; } // namespace steering_controllers_library diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 2bea1d47f8..3ff3565f58 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -129,7 +129,7 @@ class SteeringOdometry * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call */ - void update_open_loop(const double v_bx, const double omega_bz, const double dt); + void update_open_loop(const double v_bx, const double omega_bz, const double dt, const bool twist_input=true); /** * \brief Set odometry type @@ -184,10 +184,11 @@ class SteeringOdometry * \param v_bx Desired linear velocity of the robot in x_b-axis direction * \param omega_bz Desired angular velocity of the robot around x_z-axis * \param open_loop If false, the IK will be calculated using measured steering angle + * \param twist_input If true, then omega_bz will be interpreted as steering angle * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz, const bool open_loop); + const double v_bx, const double omega_bz, const bool open_loop, const bool twist_input=true); /** * \brief Reset poses, heading, and accumulators @@ -226,6 +227,13 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates steering angle from the desired twist + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param phi Steering angle of the robot around x_z-axis + */ + double convert_steering_angle_to_angular_velocity(const double v_bx, const double phi); + /** * \brief Calculates linear velocity of a robot with double traction axle * \param right_traction_wheel_vel Right traction wheel velocity [rad/s] diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index cf85ac3e41..82b81c28a7 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -33,6 +33,9 @@ namespace using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +using ControllerAckermannReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; + // called from RT control loop void reset_controller_reference_msg( const std::shared_ptr & msg, @@ -49,6 +52,18 @@ void reset_controller_reference_msg( } // namespace +void reset_controller_reference_msg( + const std::shared_ptr & msg, + const std::shared_ptr & node) +{ + msg->header.stamp = node->now(); + msg->drive.speed = std::numeric_limits::quiet_NaN(); + msg->drive.acceleration = std::numeric_limits::quiet_NaN(); + msg->drive.jerk = std::numeric_limits::quiet_NaN(); + msg->drive.steering_angle = std::numeric_limits::quiet_NaN(); + msg->drive.steering_angle_velocity = std::numeric_limits::quiet_NaN(); +} + namespace steering_controllers_library { SteeringControllersLibrary::SteeringControllersLibrary() @@ -114,12 +129,20 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.use_stamped_vel) + if (params_.twist_input && params_.use_stamped_vel) { ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); } + else if (!params_.twist_input) + { + ref_subscriber_ackermann_ = + get_node()->create_subscription( + "~/reference_ackermann", subscribers_qos, + std::bind( + &SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1)); + } else { RCLCPP_WARN( @@ -131,10 +154,20 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); } - std::shared_ptr msg = - std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + if (params_.twist_input) + { + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + } + else + { + std::shared_ptr msg = + std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ackermann_ref_.writeFromNonRT(msg); + } try { @@ -244,6 +277,34 @@ void SteeringControllersLibrary::reference_callback( } } +void SteeringControllersLibrary::reference_callback_ackermann( + const std::shared_ptr msg) +{ + // if no timestamp provided use current time for command timestamp + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Timestamp in header is missing, using current time as command timestamp."); + msg->header.stamp = get_node()->now(); + } + const auto age_of_last_command = get_node()->now() - msg->header.stamp; + + if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) + { + input_ackermann_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " + "(%.4f).", + rclcpp::Time(msg->header.stamp).seconds(), age_of_last_command.seconds(), + ref_timeout_.seconds()); + } +} + void SteeringControllersLibrary::reference_callback_unstamped( const std::shared_ptr msg) { @@ -389,7 +450,12 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + if(params_.twist_input){ + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + } + else{ + reset_controller_reference_msg(*(input_ackermann_ref_.readFromRT()), get_node()); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -407,26 +473,52 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { - auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; + if(params_.twist_input){ + auto current_ref = *(input_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + { + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; + } + } + else { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); + current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + } } } - else - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + else { + auto current_ref = *(input_ackermann_ref_.readFromRT()); + const auto age_of_last_command = time - (current_ref)->header.stamp; + + // send message only if there is no timeout + if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); + if (!std::isnan(current_ref->drive.speed) && !std::isnan(current_ref->drive.steering_angle)) + { + reference_interfaces_[0] = current_ref->drive.speed; + reference_interfaces_[1] = current_ref->drive.steering_angle; + } + } + else + { + if (!std::isnan(current_ref->drive.speed) && !std::isnan(current_ref->drive.steering_angle)) + { + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + current_ref->drive.speed = std::numeric_limits::quiet_NaN(); + current_ref->drive.steering_angle = std::numeric_limits::quiet_NaN(); + } } } @@ -447,10 +539,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { // store and set commands last_linear_velocity_ = reference_interfaces_[0]; - last_angular_velocity_ = reference_interfaces_[1]; - + last_angular_command_ = reference_interfaces_[1]; + auto [traction_commands, steering_commands] = - odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); + odometry_.get_commands(last_linear_velocity_, last_angular_command_, params_.open_loop, params_.twist_input); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index b18cac5ae1..4bd41bd98d 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -63,6 +63,13 @@ steering_controllers_library: read_only: false, } + twist_input: { + type: bool, + default_value: true, + description: "Choose whether a Twist/TwistStamped or a AckermannDriveStamped message is used as input.", + read_only: false, + } + velocity_rolling_window_size: { type: int, default_value: 10, diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 97ef60a708..78fa696e42 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -175,11 +175,11 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } -void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) +void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt, const bool twist_input) { /// Save last linear and angular velocity: linear_ = v_bx; - angular_ = omega_bz; + angular_ = twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz); /// Integrate odometry: integrate_fk(v_bx, omega_bz, dt); @@ -211,8 +211,13 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome return std::atan(omega_bz * wheelbase_ / v_bx); } +double SteeringOdometry::convert_steering_angle_to_angular_velocity(double v_bx, double phi) +{ + return std::tan(phi) * v_bx / wheelbase_; +} + std::tuple, std::vector> SteeringOdometry::get_commands( - const double v_bx, const double omega_bz, const bool open_loop) + const double v_bx, const double omega_bz, const bool open_loop, const bool twist_input) { // desired wheel speed and steering angle of the middle of traction and steering axis double Ws, phi, phi_IK = steer_pos_; @@ -232,6 +237,11 @@ std::tuple, std::vector> SteeringOdometry::get_comma #endif // steering angle phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + // interprete twist input as steering angle if twist_input is false + if (!twist_input) + { + phi = omega_bz; + } if (open_loop) { phi_IK = phi; diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 03be6b88f0..905bd01226 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -56,7 +56,7 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period { if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + odometry_.update_open_loop(last_linear_velocity_, last_angular_command_, period.seconds(), params_.twist_input); } else { From ac004d0ae0a264f2d70bece7f3536aefb8df5964 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 18 Jun 2024 10:21:17 +0200 Subject: [PATCH 08/13] [Steering controllers library] Reference interfaces are body twist (#1168) --- .../test/test_ackermann_steering_controller.cpp | 2 +- .../test/test_ackermann_steering_controller.hpp | 2 +- .../test/test_bicycle_steering_controller.cpp | 2 +- .../test/test_bicycle_steering_controller.hpp | 2 +- steering_controllers_library/doc/userdoc.rst | 4 +++- .../steering_controllers_library.hpp | 2 +- .../src/steering_controllers_library.cpp | 4 ++-- .../test/test_steering_controllers_library.cpp | 2 +- .../test/test_steering_controllers_library.hpp | 2 +- .../test/test_tricycle_steering_controller.hpp | 2 +- 10 files changed, 13 insertions(+), 11 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index e4f0aae146..718e6f5856 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -84,7 +84,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 0352a79cab..c28e692b78 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -301,7 +301,7 @@ class AckermannSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index db7a9da67e..9904f791b6 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -68,7 +68,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index b82509440b..0c894087be 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -265,7 +265,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {3.3, 0.5}; std::array joint_command_values_ = {1.1, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index a52e34c120..44b180162e 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -56,7 +56,9 @@ References (from a preceding controller) Used when controller is in chained mode (``in_chained_mode == true``). - ``/linear/velocity`` double, in m/s -- ``/angular/position`` double, in rad +- ``/angular/velocity`` double, in rad/s + +representing the body twist. Command interfaces ,,,,,,,,,,,,,,,,,,, diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index d0f00dd51d..d8f2472220 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -133,7 +133,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // name constants for reference interfaces size_t nr_ref_itfs_; - // store last velocity + // last velocity commands for open loop odometry double last_linear_velocity_ = 0.0; double last_angular_command_ = 0.0; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 82b81c28a7..2cb631fff5 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -434,7 +434,7 @@ SteeringControllersLibrary::on_export_reference_interfaces() &reference_interfaces_[0])); reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_POSITION, + get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[1])); return reference_interfaces; @@ -537,7 +537,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - // store and set commands + // store (for open loop odometry) and set commands last_linear_velocity_ = reference_interfaces_[0]; last_angular_command_ = reference_interfaces_[1]; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 0217567a26..98ab97fdc3 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -66,7 +66,7 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME + // check ref itfs auto reference_interfaces = controller_->export_reference_interfaces(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 8a0d21cc6c..edc4575176 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -323,7 +323,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index c2528e1fc9..278994a4f3 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -284,7 +284,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test std::array joint_state_values_ = {0.5, 0.5, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2}; - std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; From 8b9a6046a56a7dccc549e80fc8b34b23755819d3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 19 Jun 2024 17:46:13 +0000 Subject: [PATCH 09/13] Fix method name --- steering_controllers_library/src/steering_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 78fa696e42..3c68db27be 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -147,7 +147,7 @@ bool SteeringOdometry::update_from_velocity( const double steer_pos, const double dt) { steer_pos_ = steer_pos; - double linear_velocity = get_lin_velocity_double_traction_axle( + double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; From 0b0f35805391516bd5d936bfe95ba6f93c1e65eb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 19 Jun 2024 20:23:33 +0000 Subject: [PATCH 10/13] Add helper function --- .../steering_controllers_library/steering_odometry.hpp | 4 ++++ steering_controllers_library/src/steering_odometry.cpp | 6 +++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 3ff3565f58..a40e31f33c 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -18,6 +18,7 @@ #ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ #define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_ +#include #include #include @@ -31,6 +32,9 @@ namespace steering_odometry const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; + +inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; } + /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 3c68db27be..9c5b237cbd 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -260,7 +260,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma std::vector traction_commands; std::vector steering_commands; // double-traction axle - if (fabs(phi_IK) < 1e-6) + if (is_close_to_zero(phi_IK)) { // avoid division by zero traction_commands = {Ws, Ws}; @@ -280,7 +280,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma { std::vector traction_commands; std::vector steering_commands; - if (fabs(phi_IK) < 1e-6) + if (is_close_to_zero(phi_IK)) { // avoid division by zero traction_commands = {Ws, Ws}; @@ -337,7 +337,7 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double delta_x_b = v_bx * dt; const double delta_theta = omega_bz * dt; - if (fabs(delta_theta) < 1e-6) + if (is_close_to_zero(delta_theta)) { /// Runge-Kutta 2nd Order (should solve problems when omega_bz is zero): integrate_runge_kutta_2(v_bx, omega_bz, dt); From 8b1d9ec2550a45e702b0e26bc61a934eefb64f3a Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 20 Jun 2024 21:22:31 +0000 Subject: [PATCH 11/13] Add default value to avoid breaking API --- .../include/steering_controllers_library/steering_odometry.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index a40e31f33c..d7c2d263e1 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -192,7 +192,7 @@ class SteeringOdometry * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( - const double v_bx, const double omega_bz, const bool open_loop, const bool twist_input=true); + const double v_bx, const double omega_bz, const bool open_loop = true, const bool twist_input=true); /** * \brief Reset poses, heading, and accumulators From ec1aa1221aeaba7362ea38706b80ea4be90b0546 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Tue, 2 Jul 2024 14:33:51 +0000 Subject: [PATCH 12/13] Add optional front wheel state feedback --- .../steering_odometry.hpp | 4 ++-- .../src/steering_controllers_library.cpp | 15 +++++++++++++-- .../src/steering_controllers_library.yaml | 7 +++++++ .../src/steering_odometry.cpp | 4 ++-- 4 files changed, 24 insertions(+), 6 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index d7c2d263e1..1c0432a823 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -232,7 +232,7 @@ class SteeringOdometry double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); /** - * \brief Calculates steering angle from the desired twist + * \brief Calculates angular velocity from the desired steering angle * \param v_bx Linear velocity of the robot in x_b-axis direction * \param phi Steering angle of the robot around x_z-axis */ @@ -244,7 +244,7 @@ class SteeringOdometry * \param left_traction_wheel_vel Left traction wheel velocity [rad/s] * \param steer_pos Steer wheel position [rad] */ - double get_lin_velocity_double_traction_axle( + double get_linear_velocity_double_traction_axle( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos); diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 2cb631fff5..8c1a006f55 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -405,10 +405,21 @@ SteeringControllersLibrary::state_interface_configuration() const } else { - for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + if(params_.use_front_wheel_feedback) + { + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + front_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } + } + else + { + for (size_t i = 0; i < front_wheels_state_names_.size(); i++) { state_interfaces_config.names.push_back( - front_wheels_state_names_[i] + "/" + traction_wheels_feedback); + rear_wheels_state_names_[i] + "/" + traction_wheels_feedback); + } } for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 4bd41bd98d..83a5373bdd 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -120,6 +120,13 @@ steering_controllers_library: read_only: false, } + use_front_wheel_feedback: { + type: bool, + default_value: false, + description: "Choice which wheels to use for feedback, if front_wheel_feedback is false then rear wheels are used for feedback", + read_only: false, + } + use_stamped_vel: { type: bool, default_value: false, diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 9c5b237cbd..6397cf05a5 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -129,7 +129,7 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } -double SteeringOdometry::get_lin_velocity_double_traction_axle( +double SteeringOdometry::get_linear_velocity_double_traction_axle( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos) { @@ -168,7 +168,7 @@ bool SteeringOdometry::update_from_velocity( (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; - double linear_velocity = get_lin_velocity_double_traction_axle( + double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; From ce1d201384becf76cb99226bac84c830adf43ba7 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Wed, 31 Jul 2024 07:38:12 +0000 Subject: [PATCH 13/13] Emergency fix for phi_IK --- steering_controllers_library/src/steering_odometry.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 3686dfc05a..546b9a365f 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -237,6 +237,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma #endif // interprete twist input as steering angle if twist_input is false phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz): omega_bz; + phi_IK = phi; if (open_loop) { phi_IK = phi;