From 768ef154827da70daeda73a25211fbdc4759ee50 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sat, 1 Mar 2025 08:50:48 +0000 Subject: [PATCH 01/26] Add additional tests --- .../test_ackermann_steering_controller.cpp | 8 +- .../test/test_bicycle_steering_controller.cpp | 8 +- steering_controllers_library/CMakeLists.txt | 10 + .../steering_controllers_library.hpp | 9 +- .../steering_odometry.hpp | 17 +- .../src/steering_controllers_library.cpp | 84 ++++- .../src/steering_controllers_library.yaml | 7 + .../src/steering_odometry.cpp | 22 +- ..._controllers_library_ackermann_params.yaml | 17 + .../steering_controllers_library_params.yaml | 1 + .../test_steering_controllers_library.cpp | 20 +- .../test_steering_controllers_library.hpp | 12 +- ...steering_controllers_library_ackermann.cpp | 202 +++++++++++ ...steering_controllers_library_ackermann.hpp | 336 ++++++++++++++++++ .../test/test_steering_odometry.cpp | 9 +- .../test_tricycle_steering_controller.cpp | 8 +- 16 files changed, 709 insertions(+), 61 deletions(-) create mode 100644 steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml create mode 100644 steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp create mode 100644 steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index c434f73467..858d72c877 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -103,7 +103,7 @@ TEST_F(AckermannSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); + auto msg = controller_->input_ref_twist_.readFromNonRT(); EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); @@ -165,7 +165,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) msg->header.stamp = controller_->get_node()->now(); msg->twist.linear.x = 0.1; msg->twist.angular.z = 0.2; - controller_->input_ref_.writeFromNonRT(msg); + controller_->input_ref_twist_.writeFromNonRT(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -185,7 +185,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -225,7 +225,7 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 7ec0983f93..f81e75e5f1 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -87,7 +87,7 @@ TEST_F(BicycleSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); + auto msg = controller_->input_ref_twist_.readFromNonRT(); EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); @@ -149,7 +149,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) msg->header.stamp = controller_->get_node()->now(); msg->twist.linear.x = 0.1; msg->twist.angular.z = 0.2; - controller_->input_ref_.writeFromNonRT(msg); + controller_->input_ref_twist_.writeFromNonRT(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -161,7 +161,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -193,7 +193,7 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 2e80ed198f..9f09df7764 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -71,6 +71,16 @@ if(BUILD_TESTING) controller_interface hardware_interface ) + add_rostest_with_parameters_gmock( + test_steering_controllers_library_ackermann test/test_steering_controllers_library_ackermann.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_ackermann_params.yaml) + target_include_directories(test_steering_controllers_library_ackermann PRIVATE include) + target_link_libraries(test_steering_controllers_library_ackermann steering_controllers_library) + ament_target_dependencies( + test_steering_controllers_library_ackermann + controller_interface + hardware_interface + ) ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) target_link_libraries(test_steering_odometry steering_controllers_library) 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 786132d2b0..3ef7d66298 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 @@ -23,6 +23,7 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "rclcpp/duration.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" @@ -85,8 +86,9 @@ 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; - realtime_tools::RealtimeBuffer> input_ref_; + rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_twist_; + realtime_tools::RealtimeBuffer> input_ref_ackermann_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; @@ -128,7 +130,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl private: // callback for topic interface - void reference_callback(const std::shared_ptr msg); + void reference_callback_twist(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 ddf9fcdec8..beb9edcb71 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -136,8 +136,9 @@ class SteeringOdometry * \param v_bx Linear velocity [m/s] * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call + * \param twist_input If true, the input is twist, otherwise it is steering angle */ - 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 @@ -193,12 +194,14 @@ class SteeringOdometry * \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 reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle + * \param twist_input If true, the input is twist, otherwise it is steering angle * has been reached * \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 = true, - const bool reduce_wheel_speed_until_steering_reached = false); + const bool reduce_wheel_speed_until_steering_reached = false, + const bool twist_input = true); /** * \brief Reset poses, heading, and accumulators @@ -212,7 +215,7 @@ class SteeringOdometry * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call */ - bool update_odometry(const double v_bx, const double omega_bz, const double dt); + bool update_odometry(const double v_bx, const double omega_bz, const double dt, const bool twist_input = true); /** * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta @@ -237,6 +240,14 @@ class SteeringOdometry */ double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); + /** + * \brief Calculates angular velocity from the desired steering angle + * \param v_bx Desired linear velocity of the robot in x_b-axis direction + * \param phi Desired steering angle + */ + double convert_steering_angle_to_angular_velocity(double v_bx, 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 a91cb87105..36d62f465f 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -28,10 +28,12 @@ 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, + const std::shared_ptr & msg, const std::shared_ptr & node) { msg->header.stamp = node->now(); @@ -43,6 +45,18 @@ void reset_controller_reference_msg( msg->twist.angular.z = std::numeric_limits::quiet_NaN(); } +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 namespace steering_controllers_library @@ -111,14 +125,22 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - ref_subscriber_twist_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&SteeringControllersLibrary::reference_callback, 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) { + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback_twist, this, std::placeholders::_1)); + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_twist_.writeFromNonRT(msg); + } else { + ref_subscriber_ackermann_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1)); + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_ackermann_.writeFromNonRT(msg); + } try { @@ -200,7 +222,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -void SteeringControllersLibrary::reference_callback( +void SteeringControllersLibrary::reference_callback_twist( const std::shared_ptr msg) { // if no timestamp provided use current time for command timestamp @@ -215,7 +237,7 @@ void SteeringControllersLibrary::reference_callback( if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { - input_ref_.writeFromNonRT(msg); + input_ref_twist_.writeFromNonRT(msg); } else { @@ -228,6 +250,11 @@ void SteeringControllersLibrary::reference_callback( } } +void SteeringControllersLibrary::reference_callback_ackermann( + const std::shared_ptr msg) +{ +} + controller_interface::InterfaceConfiguration SteeringControllersLibrary::command_interface_configuration() const { @@ -337,7 +364,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(ref_subscriber_twist_ != nullptr){ + reset_controller_reference_msg(*(input_ref_twist_.readFromRT()), get_node()); + } + if(ref_subscriber_ackermann_ != nullptr){ + reset_controller_reference_msg(*(input_ref_ackermann_.readFromRT()), get_node()); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -355,12 +387,23 @@ 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()); - - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) + if(params_.twist_input) + { + auto current_ref = *(input_ref_twist_.readFromRT()); + 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; + auto current_ref = *(input_ref_ackermann_.readFromRT()); + 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; + } } return controller_interface::return_type::OK; @@ -378,7 +421,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; + const auto age_of_last_command = params_.twist_input ? + time - (*(input_ref_twist_.readFromRT()))->header.stamp : + time - (*(input_ref_ackermann_.readFromRT()))->header.stamp; + const auto timeout = age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); @@ -388,7 +434,9 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, - params_.reduce_wheel_speed_until_steering_reached); + params_.reduce_wheel_speed_until_steering_reached, + params_.twist_input + ); if (params_.front_steering) { diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 4e7deef698..7eb8c4abdb 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -119,3 +119,10 @@ steering_controllers_library: position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", 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, + } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index b2bf100255..3c07de4799 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -51,10 +51,14 @@ void SteeringOdometry::init(const rclcpp::Time & time) } bool SteeringOdometry::update_odometry( - const double linear_velocity, const double angular_velocity, const double dt) + const double linear_velocity, const double angular_velocity, const double dt, const bool twist_input) { /// Integrate odometry: - integrate_fk(linear_velocity, angular_velocity, dt); + integrate_fk( + linear_velocity, + twist_input ? angular_velocity : convert_steering_angle_to_angular_velocity(linear_velocity, angular_velocity), + dt + ); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -180,11 +184,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); @@ -216,9 +220,15 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome return std::isfinite(phi) ? phi : 0.0; } +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 bool reduce_wheel_speed_until_steering_reached) + const bool reduce_wheel_speed_until_steering_reached, + 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_; @@ -237,7 +247,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma } #endif // steering angle - phi = SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz); + phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz): omega_bz; if (open_loop) { phi_IK = phi; diff --git a/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml b/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml new file mode 100644 index 0000000000..cad7c030cf --- /dev/null +++ b/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml @@ -0,0 +1,17 @@ +test_steering_controllers_library: + ros__parameters: + + reference_timeout: 0.1 + front_steering: true + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + twist_input: false + rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + + wheelbase: 3.24644 + front_wheel_track: 2.12321 + rear_wheel_track: 1.76868 + front_wheels_radius: 0.45 + rear_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index 01bfb02302..c18f82cdde 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -6,6 +6,7 @@ test_steering_controllers_library: open_loop: false velocity_rolling_window_size: 10 position_feedback: false + twist_input: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 3378efbef8..720b8c5fc4 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -101,7 +101,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) const double TEST_LINEAR_VELOCITY_Y = 0.0; const double TEST_ANGULAR_VELOCITY_Z = 0.3; - std::shared_ptr msg = std::make_shared(); + std::shared_ptr msg = std::make_shared(); // adjusting to achieve age_of_last_command > ref_timeout msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - @@ -112,17 +112,17 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) msg->twist.angular.x = std::numeric_limits::quiet_NaN(); msg->twist.angular.y = std::numeric_limits::quiet_NaN(); msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); + controller_->input_ref_twist_.writeFromNonRT(msg); const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_.readFromNonRT()))->header.stamp; + controller_->get_node()->now() - (*(controller_->input_ref_twist_.readFromNonRT()))->header.stamp; // case 1 position_feedback = false controller_->params_.position_feedback = false; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -133,8 +133,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + ASSERT_EQ((*(controller_->input_ref_twist_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_twist_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -162,11 +162,11 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) msg->twist.angular.x = std::numeric_limits::quiet_NaN(); msg->twist.angular.y = std::numeric_limits::quiet_NaN(); msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; - controller_->input_ref_.writeFromNonRT(msg); + controller_->input_ref_twist_.writeFromNonRT(msg); // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -177,8 +177,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + ASSERT_EQ((*(controller_->input_ref_twist_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_twist_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 20d136ab32..9e5ac7f230 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -33,8 +33,10 @@ using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; -using ControllerReferenceMsg = +using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +using ControllerAckermannReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; // NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only @@ -89,7 +91,7 @@ class TestableSteeringControllersLibrary } /** - * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * @brief wait_for_command blocks until a new ControllerTwistReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. */ @@ -143,7 +145,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test controller_ = std::make_unique(); command_publisher_node_ = std::make_shared("command_publisher"); - command_publisher_ = command_publisher_node_->create_publisher( + command_publisher_ = command_publisher_node_->create_publisher( "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); } @@ -280,7 +282,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test wait_for_topic(command_publisher_->get_topic_name()); - ControllerReferenceMsg msg; + ControllerTwistReferenceMsg msg; msg.twist.linear.x = linear; msg.twist.angular.z = angular; @@ -330,7 +332,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test // Test related parameters std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; - rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Publisher::SharedPtr command_publisher_; }; #endif // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp new file mode 100644 index 0000000000..5158a22c20 --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -0,0 +1,202 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "test_steering_controllers_library_ackermann.hpp" + +#include +#include +#include +#include + +class SteeringControllersLibraryTest +: public SteeringControllersLibraryFixture +{ +}; + +// checking if all interfaces, command, state and reference are exported as expected +TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], + rear_wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], + rear_wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], + front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], + controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], + controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], + controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_LEFT_WHEEL], + controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // 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) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + } +} + +// Tests controller update_reference_from_subscribers and +// its two cases for position_feedback true/false behavior +// when too old msg is sent i.e age_of_last_command > ref_timeout case +TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // set command statically + const double TEST_LINEAR_VELOCITY_X = 1.5; + const float TEST_STEERING_ANGLE = (float)0.575875; + + std::shared_ptr msg = std::make_shared(); + + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->drive.speed = TEST_LINEAR_VELOCITY_X; + msg->drive.steering_angle = TEST_STEERING_ANGLE; + msg->drive.steering_angle_velocity = std::numeric_limits::quiet_NaN(); + msg->drive.acceleration = std::numeric_limits::quiet_NaN(); + msg->drive.jerk = std::numeric_limits::quiet_NaN(); + controller_->input_ref_ackermann_.writeFromNonRT(msg); + + const auto age_of_last_command = + controller_->get_node()->now() - (*(controller_->input_ref_ackermann_.readFromNonRT()))->header.stamp; + + // case 1 position_feedback = false + controller_->params_.position_feedback = false; + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle, TEST_STEERING_ANGLE); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + + // case 2 position_feedback = true + controller_->params_.position_feedback = true; + + // adjusting to achieve age_of_last_command > ref_timeout + msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - + rclcpp::Duration::from_seconds(0.1); + msg->drive.speed = TEST_LINEAR_VELOCITY_X; + msg->drive.steering_angle = TEST_STEERING_ANGLE; + msg->drive.steering_angle_velocity = std::numeric_limits::quiet_NaN(); + msg->drive.acceleration = std::numeric_limits::quiet_NaN(); + msg->drive.jerk = std::numeric_limits::quiet_NaN(); + controller_->input_ref_ackermann_.writeFromNonRT(msg); + + // age_of_last_command > ref_timeout_ + ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); + ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle, TEST_STEERING_ANGLE); + + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp new file mode 100644 index 0000000000..2ac3681b1e --- /dev/null +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp @@ -0,0 +1,336 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; +using ControllerAckermannReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; + +// NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only + +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2; +static constexpr size_t STATE_STEER_LEFT_WHEEL = 3; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1; +static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2; +static constexpr size_t CMD_STEER_LEFT_WHEEL = 3; + +static constexpr size_t NR_STATE_ITFS = 4; +static constexpr size_t NR_CMD_ITFS = 4; +static constexpr size_t NR_REF_ITFS = 2; + +static constexpr double WHEELBASE_ = 3.24644; +static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; +static constexpr double REAR_WHEEL_TRACK_ = 1.76868; +static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; +static constexpr double REAR_WHEELS_RADIUS_ = 0.45; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace +// namespace + +// subclassing and friending so we can access member variables +class TestableSteeringControllersLibrary +: public steering_controllers_library::SteeringControllersLibrary +{ + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); + FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return steering_controllers_library::SteeringControllersLibrary::on_configure(previous_state); + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return steering_controllers_library::SteeringControllersLibrary::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerAckermannReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + wait_for_command(executor, timeout); + } + + // implementing methods which are declared virtual in the steering_controllers_library.hpp + void initialize_implementation_parameter_listener() + { + param_listener_ = std::make_shared(get_node()); + } + + controller_interface::CallbackReturn configure_odometry() + { + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); + odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); + + return controller_interface::CallbackReturn::SUCCESS; + } + + bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class SteeringControllersLibraryFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_steering_controllers_library") + { + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + ControllerStateMsg::SharedPtr received_msg; + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; + auto subscription = test_subscription_node.create_subscription( + "/test_steering_controllers_library/controller_state", 10, subs_callback); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_subscription_node.get_node_base_interface()); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = test_subscription_node.get_clock()->now() + timeout; + while (!received_msg && test_subscription_node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (received_msg.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(received_msg); + + // take message from subscription + msg = *received_msg; + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerAckermannReferenceMsg msg; + msg.drive.speed = linear; + msg.drive.steering_angle = angular; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + double reference_timeout_ = 2.0; + bool front_steering_ = true; + bool open_loop_ = false; + unsigned int velocity_rolling_window_size_ = 10; + bool position_feedback_ = false; + std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector front_wheels_names_ = { + "front_right_steering_joint", "front_left_steering_joint"}; + std::vector joint_names_ = { + rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + + std::vector rear_wheels_preceeding_names_ = { + "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector front_wheels_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], + front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + + double wheelbase_ = 3.24644; + double front_wheel_track_ = 2.12321; + double rear_wheel_track_ = 1.76868; + double front_wheels_radius_ = 0.45; + double rear_wheels_radius_ = 0.45; + + 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/velocity"}}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3d6d97f15f..17d7958884 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -66,16 +66,17 @@ TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } -TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) +TEST(TestSteeringOdometry, ackermann_odometry_openloop_ackermanndrive_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.); + odom.update_open_loop(1., 1., 1., false); EXPECT_DOUBLE_EQ(odom.get_linear(), 1.); - EXPECT_DOUBLE_EQ(odom.get_angular(), -1.); + double expected_angular = (1.0 / 2.0) * std::tan(1.0); + EXPECT_NEAR(odom.get_angular(), expected_angular, 1e-6); EXPECT_GT(odom.get_x(), 0); // pos x - EXPECT_LT(odom.get_y(), 0); // neg y ie. right + EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } TEST(TestSteeringOdometry, ackermann_IK_linear) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 8e29314f8e..65b6a7bda4 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -95,7 +95,7 @@ TEST_F(TricycleSteeringControllerTest, activate_success) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); + auto msg = controller_->input_ref_twist_.readFromNonRT(); EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); @@ -157,7 +157,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) msg->header.stamp = controller_->get_node()->now(); msg->twist.linear.x = 0.1; msg->twist.angular.z = 0.2; - controller_->input_ref_.writeFromNonRT(msg); + controller_->input_ref_twist_.writeFromNonRT(msg); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -173,7 +173,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_FALSE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { @@ -210,7 +210,7 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x)); EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); for (const auto & interface : controller_->reference_interfaces_) { From 36bc9d54ef7a92648de81ded6134e00d7977b65b Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sat, 1 Mar 2025 10:07:34 +0000 Subject: [PATCH 02/26] Remove twist_input from closed loop odometry calculation --- .../src/ackermann_steering_controller.cpp | 2 +- .../src/bicycle_steering_controller.cpp | 2 +- .../steering_controllers_library/steering_odometry.hpp | 2 +- steering_controllers_library/src/steering_odometry.cpp | 4 ++-- .../src/tricycle_steering_controller.cpp | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 77ba55812a..4095380854 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_velocity_, 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..6eb863c765 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_velocity_, period.seconds(), params_.twist_input); } else { 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 beb9edcb71..0549235ac3 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -215,7 +215,7 @@ class SteeringOdometry * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call */ - bool update_odometry(const double v_bx, const double omega_bz, const double dt, const bool twist_input = true); + bool update_odometry(const double v_bx, const double omega_bz, const double dt); /** * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 69453a5f77..0d3d7822e6 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -53,12 +53,12 @@ void SteeringOdometry::init(const rclcpp::Time & time) } bool SteeringOdometry::update_odometry( - const double linear_velocity, const double angular_velocity, const double dt, const bool twist_input) + const double linear_velocity, const double angular_velocity, const double dt) { /// Integrate odometry: integrate_fk( linear_velocity, - twist_input ? angular_velocity : convert_steering_angle_to_angular_velocity(linear_velocity, angular_velocity), + angular_velocity, dt ); diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 03be6b88f0..97cb11085a 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_velocity_, period.seconds(), params_.twist_input); } else { From c0f103e4c17181363bfcb7c2d7aae39e0134dced Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sat, 1 Mar 2025 10:19:09 +0000 Subject: [PATCH 03/26] Add in missing test case --- .../test/test_steering_odometry.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 585d19bdd2..88983a22c6 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -68,6 +68,19 @@ TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_left) EXPECT_GT(odom.get_y(), 0); // pos y, ie. left } +TEST(TestSteeringOdometry, ackermann_odometry_openloop_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. left +} + TEST(TestSteeringOdometry, ackermann_odometry_openloop_ackermanndrive_angular_left) { steering_odometry::SteeringOdometry odom(1); From eaebf092f0a47911244f995ea7b7ba58f0881fac Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sat, 1 Mar 2025 10:20:52 +0000 Subject: [PATCH 04/26] Fixed comment --- steering_controllers_library/test/test_steering_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 88983a22c6..b33655779b 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -78,7 +78,7 @@ TEST(TestSteeringOdometry, ackermann_odometry_openloop_angular_right) EXPECT_DOUBLE_EQ(odom.get_angular(), -1.); EXPECT_GT(odom.get_x(), 0); // pos x - EXPECT_LT(odom.get_y(), 0); // neg y, ie. left + EXPECT_LT(odom.get_y(), 0); // neg y, ie. right } TEST(TestSteeringOdometry, ackermann_odometry_openloop_ackermanndrive_angular_left) From 3e84eddf484549df93c382695524f635da7c1cdc Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sat, 1 Mar 2025 10:48:05 +0000 Subject: [PATCH 05/26] Update on ackermann message --- .../src/steering_controllers_library.cpp | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index b6f4b9a047..66f4d152e2 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -253,6 +253,29 @@ void SteeringControllersLibrary::reference_callback_twist( 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_ref_ackermann_.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()); + } } controller_interface::InterfaceConfiguration From 0c3ded57cc39939a31193eaac853b211e889252c Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sun, 6 Apr 2025 19:41:11 +0200 Subject: [PATCH 06/26] Fix reference name --- .../src/steering_controllers_library.cpp | 9 ++++++--- .../src/steering_odometry.cpp | 6 +++--- ...est_steering_controllers_library_ackermann.cpp | 15 +++++++++------ 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 66f4d152e2..5cd8379d83 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -369,9 +369,10 @@ SteeringControllersLibrary::on_export_reference_interfaces() reference_interfaces.push_back(hardware_interface::CommandInterface( get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[0])); + reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY, + get_node()->get_name() + std::string("/angular"), params_.twist_input ? hardware_interface::HW_IF_VELOCITY: hardware_interface::HW_IF_POSITION, &reference_interfaces_[1])); return reference_interfaces; @@ -387,10 +388,12 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - if(ref_subscriber_twist_ != nullptr){ + if(params_.twist_input) + { reset_controller_reference_msg(*(input_ref_twist_.readFromRT()), get_node()); } - if(ref_subscriber_ackermann_ != nullptr){ + else + { reset_controller_reference_msg(*(input_ref_ackermann_.readFromRT()), get_node()); } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 0d3d7822e6..4913d24b88 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( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos); return update_odometry(linear_velocity, angular_velocity, dt); } @@ -161,7 +161,7 @@ bool SteeringOdometry::update_from_velocity( 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_; + const double angular_velocity = convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); return update_odometry(linear_velocity, angular_velocity, dt); } @@ -181,7 +181,7 @@ bool SteeringOdometry::update_from_velocity( 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_; + const double angular_velocity = convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); return update_odometry(linear_velocity, angular_velocity, dt); } diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index 5158a22c20..95a9181f35 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_steering_controllers_library_ackermann.hpp" - #include #include #include #include +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "test_steering_controllers_library_ackermann.hpp" + class SteeringControllersLibraryTest : public SteeringControllersLibraryFixture { @@ -68,11 +69,13 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { - const std::string ref_itf_name = + const std::string ref_itf_prefix_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); + EXPECT_EQ( + reference_interfaces[i]->get_name(), + ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); } } From d204e7e90808e5342d8fc35c754054f6df355a66 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Thu, 10 Apr 2025 11:36:53 +0000 Subject: [PATCH 07/26] Fix typo --- .../src/steering_controllers_library.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f08a2e3e8d..267eca4b2e 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -373,7 +373,7 @@ SteeringControllersLibrary::on_export_reference_interfaces() reference_interfaces.push_back( hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), params_.twist_input ? hardware_interface::HW_IF_VELOCITY: hardware_interface::HW_IF_POSITION,, + get_node()->get_name() + std::string("/angular"), params_.twist_input ? hardware_interface::HW_IF_VELOCITY: hardware_interface::HW_IF_POSITION, &reference_interfaces_[1])); return reference_interfaces; From c612f3cd5e9360dacef2fd30779687ed8a15c204 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Thu, 10 Apr 2025 21:41:00 +0000 Subject: [PATCH 08/26] Fix tests --- .../src/steering_controllers_library.cpp | 2 +- ...test_steering_controllers_library_ackermann.cpp | 14 +++++++++----- ...test_steering_controllers_library_ackermann.hpp | 2 +- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 267eca4b2e..affe6405dc 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -373,7 +373,7 @@ SteeringControllersLibrary::on_export_reference_interfaces() reference_interfaces.push_back( hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), params_.twist_input ? hardware_interface::HW_IF_VELOCITY: hardware_interface::HW_IF_POSITION, + get_node()->get_name() + std::string("/angular"), (params_.twist_input ? hardware_interface::HW_IF_VELOCITY : hardware_interface::HW_IF_POSITION), &reference_interfaces_[1])); return reference_interfaces; diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index 95a9181f35..1ed23a7ca8 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -70,12 +70,16 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_prefix_name = - std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + std::string(controller_->get_node()->get_name()); EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); - EXPECT_EQ( - reference_interfaces[i]->get_name(), - ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); - EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + if(reference_interfaces[i]->get_interface_name() == hardware_interface::HW_IF_VELOCITY) + { + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); + } + else + { + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_prefix_name + "/" + hardware_interface::HW_IF_POSITION); + } } } diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp index 2ac3681b1e..6d2da89496 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp @@ -318,7 +318,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; std::array joint_reference_interfaces_ = { - {"linear/velocity", "angular/velocity"}}; + {"linear/velocity", "angular/position"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; From 915e3bd2b60a743d22ba7610d481c7ed55033856 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Fri, 11 Apr 2025 20:03:52 +0000 Subject: [PATCH 09/26] Fix exported interfaces tests --- ...test_steering_controllers_library_ackermann.cpp | 14 +++++--------- ...test_steering_controllers_library_ackermann.hpp | 2 +- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index 1ed23a7ca8..a0e4c0286f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -70,17 +70,13 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { const std::string ref_itf_prefix_name = - std::string(controller_->get_node()->get_name()); + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); - if(reference_interfaces[i]->get_interface_name() == hardware_interface::HW_IF_VELOCITY) - { - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY); - } - else - { - EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_prefix_name + "/" + hardware_interface::HW_IF_POSITION); - } } + EXPECT_EQ(reference_interfaces[0]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); + EXPECT_EQ(reference_interfaces[1]->get_interface_name(), hardware_interface::HW_IF_POSITION); + + } // Tests controller update_reference_from_subscribers and diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp index 6d2da89496..658d3c2099 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp @@ -318,7 +318,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test std::array joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}}; std::array joint_reference_interfaces_ = { - {"linear/velocity", "angular/position"}}; + {"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; From b038514280ee1a6f8fb06a26a554ae68dc90f80e Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Fri, 11 Apr 2025 20:25:25 +0000 Subject: [PATCH 10/26] Add comment --- .../test/test_steering_controllers_library_ackermann.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index a0e4c0286f..a5d7a0a2ea 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -73,6 +73,8 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name); } + + // explicitly check the names of the two reference interfaces EXPECT_EQ(reference_interfaces[0]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); EXPECT_EQ(reference_interfaces[1]->get_interface_name(), hardware_interface::HW_IF_POSITION); From b79d6825226f6fb3f9ff6f60caf865739db66bf2 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Fri, 11 Apr 2025 20:27:29 +0000 Subject: [PATCH 11/26] Ran precommit hooks --- .../src/ackermann_steering_controller.cpp | 3 +- .../src/bicycle_steering_controller.cpp | 3 +- .../steering_controllers_library.hpp | 8 ++- .../steering_odometry.hpp | 7 +-- .../src/steering_controllers_library.cpp | 43 ++++++++------ .../src/steering_odometry.cpp | 23 ++++---- .../test_steering_controllers_library.cpp | 25 +++++--- ...steering_controllers_library_ackermann.cpp | 28 +++++---- ...steering_controllers_library_ackermann.hpp | 59 +++++++++++-------- .../src/tricycle_steering_controller.cpp | 3 +- 10 files changed, 117 insertions(+), 85 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 4095380854..3893e982fc 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -58,7 +58,8 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio { if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.twist_input); + odometry_.update_open_loop( + last_linear_velocity_, last_angular_velocity_, 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 6eb863c765..bf0f70e212 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -56,7 +56,8 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) { if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.twist_input); + odometry_.update_open_loop( + last_linear_velocity_, last_angular_velocity_, 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 3ef7d66298..629a00ac47 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 @@ -22,8 +22,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "hardware_interface/handle.hpp" -#include "rclcpp_lifecycle/state.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" @@ -86,9 +86,11 @@ 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; realtime_tools::RealtimeBuffer> input_ref_twist_; - realtime_tools::RealtimeBuffer> input_ref_ackermann_; + realtime_tools::RealtimeBuffer> + input_ref_ackermann_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; 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 0549235ac3..6601632d8f 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -138,7 +138,8 @@ class SteeringOdometry * \param dt time difference to last call * \param twist_input If true, the input is twist, otherwise it is steering angle */ - void update_open_loop(const double v_bx, const double omega_bz, const double dt, const bool twist_input=true); + void update_open_loop( + const double v_bx, const double omega_bz, const double dt, const bool twist_input = true); /** * \brief Set odometry type @@ -200,8 +201,7 @@ class SteeringOdometry */ std::tuple, std::vector> get_commands( const double v_bx, const double omega_bz, const bool open_loop = true, - const bool reduce_wheel_speed_until_steering_reached = false, - const bool twist_input = true); + const bool reduce_wheel_speed_until_steering_reached = false, const bool twist_input = true); /** * \brief Reset poses, heading, and accumulators @@ -247,7 +247,6 @@ class SteeringOdometry */ double convert_steering_angle_to_angular_velocity(double v_bx, 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 affe6405dc..52b2784b9f 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -33,7 +33,7 @@ using ControllerAckermannReferenceMsg = // called from RT control loop void reset_controller_reference_msg( - const std::shared_ptr & msg, + const std::shared_ptr & msg, const std::shared_ptr & node) { msg->header.stamp = node->now(); @@ -126,18 +126,25 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.twist_input) { + if (params_.twist_input) + { ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, - std::bind(&SteeringControllersLibrary::reference_callback_twist, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); + std::bind( + &SteeringControllersLibrary::reference_callback_twist, this, std::placeholders::_1)); + std::shared_ptr msg = + std::make_shared(); reset_controller_reference_msg(msg, get_node()); input_ref_twist_.writeFromNonRT(msg); - } else { + } + else + { ref_subscriber_ackermann_ = get_node()->create_subscription( "~/reference", subscribers_qos, - std::bind(&SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1)); - std::shared_ptr msg = std::make_shared(); + std::bind( + &SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1)); + std::shared_ptr msg = + std::make_shared(); reset_controller_reference_msg(msg, get_node()); input_ref_ackermann_.writeFromNonRT(msg); } @@ -253,7 +260,7 @@ void SteeringControllersLibrary::reference_callback_twist( void SteeringControllersLibrary::reference_callback_ackermann( const std::shared_ptr msg) { - // if no timestamp provided use current time for command timestamp + // if no timestamp provided use current time for command timestamp if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0u) { RCLCPP_WARN( @@ -373,7 +380,9 @@ SteeringControllersLibrary::on_export_reference_interfaces() reference_interfaces.push_back( hardware_interface::CommandInterface( - get_node()->get_name() + std::string("/angular"), (params_.twist_input ? hardware_interface::HW_IF_VELOCITY : hardware_interface::HW_IF_POSITION), + get_node()->get_name() + std::string("/angular"), + (params_.twist_input ? hardware_interface::HW_IF_VELOCITY + : hardware_interface::HW_IF_POSITION), &reference_interfaces_[1])); return reference_interfaces; @@ -389,7 +398,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - if(params_.twist_input) + if (params_.twist_input) { reset_controller_reference_msg(*(input_ref_twist_.readFromRT()), get_node()); } @@ -414,7 +423,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - if(params_.twist_input) + if (params_.twist_input) { auto current_ref = *(input_ref_twist_.readFromRT()); if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) @@ -448,10 +457,10 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - const auto age_of_last_command = params_.twist_input ? - time - (*(input_ref_twist_.readFromRT()))->header.stamp : - time - (*(input_ref_ackermann_.readFromRT()))->header.stamp; - + const auto age_of_last_command = + params_.twist_input ? time - (*(input_ref_twist_.readFromRT()))->header.stamp + : time - (*(input_ref_ackermann_.readFromRT()))->header.stamp; + const auto timeout = age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); @@ -461,9 +470,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, - params_.reduce_wheel_speed_until_steering_reached, - params_.twist_input - ); + params_.reduce_wheel_speed_until_steering_reached, params_.twist_input); if (params_.front_steering) { diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 4913d24b88..5ce891c4d2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -56,11 +56,7 @@ bool SteeringOdometry::update_odometry( const double linear_velocity, const double angular_velocity, const double dt) { /// Integrate odometry: - integrate_fk( - linear_velocity, - angular_velocity, - dt - ); + integrate_fk(linear_velocity, angular_velocity, dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -129,7 +125,8 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos); + const double angular_velocity = + convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos); return update_odometry(linear_velocity, angular_velocity, dt); } @@ -161,7 +158,8 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); + const double angular_velocity = + convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); return update_odometry(linear_velocity, angular_velocity, dt); } @@ -181,12 +179,14 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); + const double angular_velocity = + convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); return update_odometry(linear_velocity, angular_velocity, dt); } -void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt, const bool twist_input) +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; @@ -229,8 +229,7 @@ double SteeringOdometry::convert_steering_angle_to_angular_velocity(double v_bx, std::tuple, std::vector> SteeringOdometry::get_commands( const double v_bx, const double omega_bz, const bool open_loop, - const bool reduce_wheel_speed_until_steering_reached, - const bool twist_input) + const bool reduce_wheel_speed_until_steering_reached, 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_; @@ -249,7 +248,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma } #endif // steering angle - phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz): omega_bz; + phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz) : omega_bz; if (open_loop) { phi_IK = phi; diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 793d042bed..29711a7252 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -104,7 +104,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) const double TEST_LINEAR_VELOCITY_Y = 0.0; const double TEST_ANGULAR_VELOCITY_Z = 0.3; - std::shared_ptr msg = std::make_shared(); + std::shared_ptr msg = + std::make_shared(); // adjusting to achieve age_of_last_command > ref_timeout msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - @@ -117,15 +118,16 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) msg->twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_twist_.writeFromNonRT(msg); - const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_twist_.readFromNonRT()))->header.stamp; + const auto age_of_last_command = controller_->get_node()->now() - + (*(controller_->input_ref_twist_.readFromNonRT()))->header.stamp; // case 1 position_feedback = false controller_->params_.position_feedback = false; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + (*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -136,8 +138,10 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_twist_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_twist_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + ASSERT_EQ( + (*(controller_->input_ref_twist_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + (*(controller_->input_ref_twist_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -169,7 +173,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + (*(controller_->input_ref_twist_.readFromRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -180,8 +185,10 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_twist_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_twist_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); + ASSERT_EQ( + (*(controller_->input_ref_twist_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + (*(controller_->input_ref_twist_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index a5d7a0a2ea..24735bbf18 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -77,8 +77,6 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) // explicitly check the names of the two reference interfaces EXPECT_EQ(reference_interfaces[0]->get_interface_name(), hardware_interface::HW_IF_VELOCITY); EXPECT_EQ(reference_interfaces[1]->get_interface_name(), hardware_interface::HW_IF_POSITION); - - } // Tests controller update_reference_from_subscribers and @@ -105,7 +103,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) const double TEST_LINEAR_VELOCITY_X = 1.5; const float TEST_STEERING_ANGLE = (float)0.575875; - std::shared_ptr msg = std::make_shared(); + std::shared_ptr msg = + std::make_shared(); // adjusting to achieve age_of_last_command > ref_timeout msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - @@ -118,14 +117,16 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) controller_->input_ref_ackermann_.writeFromNonRT(msg); const auto age_of_last_command = - controller_->get_node()->now() - (*(controller_->input_ref_ackermann_.readFromNonRT()))->header.stamp; + controller_->get_node()->now() - + (*(controller_->input_ref_ackermann_.readFromNonRT()))->header.stamp; // case 1 position_feedback = false controller_->params_.position_feedback = false; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + (*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -136,8 +137,11 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle, TEST_STEERING_ANGLE); + ASSERT_EQ( + (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle, + TEST_STEERING_ANGLE); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -168,7 +172,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + (*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -179,8 +184,11 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ((*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle, TEST_STEERING_ANGLE); + ASSERT_EQ( + (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ( + (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle, + TEST_STEERING_ANGLE); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp index 658d3c2099..e51ceccfa1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp @@ -171,48 +171,56 @@ class SteeringControllersLibraryFixture : public ::testing::Test command_itfs_.reserve(joint_command_values_.size()); command_ifs.reserve(joint_command_values_.size()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, - &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[1], steering_interface_name_, - &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + rear_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, - &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[1], steering_interface_name_, - &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; state_itfs_.reserve(joint_state_values_.size()); state_ifs.reserve(joint_state_values_.size()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[1], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface( + rear_wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, - &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface( + front_wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[1], steering_interface_name_, - &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface( + front_wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -317,8 +325,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", "angular"}}; + std::array joint_reference_interfaces_ = {{"linear", "angular"}}; std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 97cb11085a..ad1f316c77 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -56,7 +56,8 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period { if (params_.open_loop) { - odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.twist_input); + odometry_.update_open_loop( + last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.twist_input); } else { From f9f245917ada6204d15dc06471098611977710d4 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Fri, 11 Apr 2025 20:31:51 +0000 Subject: [PATCH 12/26] Fix all pre-commit checks --- .../test/test_steering_controllers_library_ackermann.cpp | 2 +- .../test/test_steering_controllers_library_ackermann.hpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index 24735bbf18..8fad5f036b 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -101,7 +101,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // set command statically const double TEST_LINEAR_VELOCITY_X = 1.5; - const float TEST_STEERING_ANGLE = (float)0.575875; + const float TEST_STEERING_ANGLE = 0.575875; std::shared_ptr msg = std::make_shared(); diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp index e51ceccfa1..e722e1c9c0 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ -#define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_ +#define TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_ #include @@ -340,4 +340,4 @@ class SteeringControllersLibraryFixture : public ::testing::Test rclcpp::Publisher::SharedPtr command_publisher_; }; -#endif // TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ +#endif // TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_ From 3782090497a426c4d4322e72082f5ae3c6015aca Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Fri, 11 Apr 2025 20:36:33 +0000 Subject: [PATCH 13/26] Update function description --- .../steering_controllers_library/steering_odometry.hpp | 6 +++--- 1 file changed, 3 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 6601632d8f..099c9aebfe 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -241,9 +241,9 @@ class SteeringOdometry double convert_twist_to_steering_angle(const double v_bx, const double omega_bz); /** - * \brief Calculates angular velocity from the desired steering angle - * \param v_bx Desired linear velocity of the robot in x_b-axis direction - * \param phi Desired steering angle + * \brief Calculates angular velocity from the steering angle and linear velocity + * \param v_bx Linear velocity of the robot in x_b-axis direction + * \param phi Steering angle */ double convert_steering_angle_to_angular_velocity(double v_bx, double phi); From abbe4fde527fd5c520432667f5c86bd5ed7e0fb3 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Mon, 14 Apr 2025 19:30:28 +0000 Subject: [PATCH 14/26] Fix missed readability issue --- .../test/test_steering_controllers_library_ackermann.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index 8fad5f036b..b4a02e7299 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -101,7 +101,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // set command statically const double TEST_LINEAR_VELOCITY_X = 1.5; - const float TEST_STEERING_ANGLE = 0.575875; + const float TEST_STEERING_ANGLE = static_cast(0.575875); std::shared_ptr msg = std::make_shared(); From 5a6e60ee43b7037c5e14e8d8ea034acdfd0a40af Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Mon, 14 Apr 2025 20:01:40 +0000 Subject: [PATCH 15/26] Switch from double to float for drive messages in tests --- .../test/test_steering_controllers_library_ackermann.cpp | 2 +- .../test/test_steering_controllers_library_ackermann.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index b4a02e7299..f29f4d6dc3 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -100,7 +100,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) } // set command statically - const double TEST_LINEAR_VELOCITY_X = 1.5; + const float TEST_LINEAR_VELOCITY_X = static_cast(1.5); const float TEST_STEERING_ANGLE = static_cast(0.575875); std::shared_ptr msg = diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp index e722e1c9c0..9b887436dc 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp @@ -268,7 +268,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test msg = *received_msg; } - void publish_commands(const double linear = 0.1, const double angular = 0.2) + void publish_commands(const float linear = 0.1, const float angular = 0.2) { auto wait_for_topic = [&](const auto topic_name) { From ed3ef219b2131dff39fa228ef978ab2a83b5461c Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Mon, 14 Apr 2025 20:28:24 +0000 Subject: [PATCH 16/26] Explicitly cast default parameters to float --- .../test/test_steering_controllers_library_ackermann.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp index 9b887436dc..d6584c4060 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp @@ -268,7 +268,8 @@ class SteeringControllersLibraryFixture : public ::testing::Test msg = *received_msg; } - void publish_commands(const float linear = 0.1, const float angular = 0.2) + void publish_commands( + const float linear = static_cast(0.1), const float angular = static_cast(0.2)) { auto wait_for_topic = [&](const auto topic_name) { From bb277290f3c31b39d987e4e8cdd611fbf5533955 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sat, 26 Apr 2025 20:29:51 +0000 Subject: [PATCH 17/26] Fixed tests again --- .../src/steering_odometry.cpp | 4 ++-- ...steering_controllers_library_ackermann.cpp | 24 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 136a08b23e..52fc248ee1 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -237,7 +237,7 @@ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double ome double SteeringOdometry::convert_steering_angle_to_angular_velocity(double v_bx, double phi) { - return std::tan(phi) * v_bx / wheelbase_; + return std::tan(phi) * v_bx / wheel_base_; } std::tuple, std::vector> SteeringOdometry::get_commands( @@ -252,7 +252,7 @@ std::tuple, std::vector> SteeringOdometry::get_comma { // 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_; + Ws = abs(omega_bz) * wheel_base_ / wheel_radius_; } else { diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp index f29f4d6dc3..eb3c6339cd 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp @@ -52,16 +52,16 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], - controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_RIGHT_WHEEL], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_LEFT_WHEEL], - controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs @@ -150,12 +150,12 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) } // Wheel velocities should reset to 0 - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0); // Steer angles should not reset - EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); // case 2 position_feedback = true controller_->params_.position_feedback = true; @@ -197,12 +197,12 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) } // Wheel velocities should reset to 0 - EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); - EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_optional().value(), 0); // Steer angles should not reset - EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); - EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[2].get_optional().value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_optional().value(), 0.575875, 1e-6); } int main(int argc, char ** argv) From 8a60aa361fcf00afae33e56c1e4436936ebfef47 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sun, 18 May 2025 12:36:23 +0000 Subject: [PATCH 18/26] Renamed message naming --- steering_controllers_library/CMakeLists.txt | 1 + .../steering_controllers_library.hpp | 2 ++ steering_controllers_library/package.xml | 1 + .../test/test_steering_controllers_library.hpp | 2 +- .../test/test_steering_controllers_library_ackermann.hpp | 2 +- 5 files changed, 6 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 003a6e117f..2bd0b3d9de 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -21,6 +21,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2 tf2_msgs tf2_geometry_msgs + ackermann_msgs ) find_package(ament_cmake REQUIRED) 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 0b97f39124..9f52fd8f3f 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 @@ -32,6 +32,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "steering_controllers_library/steering_controllers_library_parameters.hpp" #include "steering_controllers_library/steering_odometry.hpp" @@ -71,6 +72,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl const rclcpp::Time & time, const rclcpp::Duration & period) override; using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; + using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped; using ControllerStateMsgOdom = nav_msgs::msg::Odometry; using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; using SteeringControllerStateMsg = control_msgs::msg::SteeringControllerStatus; diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 1e52dd899e..4526082d5f 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -40,6 +40,7 @@ tf2 tf2_msgs tf2_geometry_msgs + ackermann_msgs ament_cmake_gmock controller_manager diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index b2acb4b7bf..0cabb075b5 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -33,7 +33,7 @@ using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg; -using ControllerReferenceMsg = +using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; using ControllerAckermannReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp index d6584c4060..6f9d7dfdfd 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp @@ -32,7 +32,7 @@ #include "steering_controllers_library/steering_controllers_library.hpp" using ControllerStateMsg = - steering_controllers_library::SteeringControllersLibrary::AckermannControllerState; + steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg; using ControllerAckermannReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; From 9c8bb76b36b9dad9d73a7b95835e4d2df2561381 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Mon, 9 Jun 2025 09:40:52 +0000 Subject: [PATCH 19/26] Rename various tests and variables --- .../src/ackermann_steering_controller.cpp | 2 +- .../src/bicycle_steering_controller.cpp | 2 +- steering_controllers_library/CMakeLists.txt | 8 +++---- steering_controllers_library/doc/userdoc.rst | 7 +++--- .../steering_controllers_library.hpp | 2 +- .../steering_odometry.hpp | 9 ++++---- .../src/steering_controllers_library.cpp | 16 +++++++------- .../src/steering_controllers_library.yaml | 6 ++--- .../src/steering_odometry.cpp | 10 +++++---- ..._controllers_library_ackermann_params.yaml | 10 ++++----- .../steering_controllers_library_params.yaml | 10 ++++----- ...g_controllers_library_ackermann_input.cpp} | 10 ++++----- ...g_controllers_library_ackermann_input.hpp} | 22 +++++++++---------- .../src/tricycle_steering_controller.cpp | 2 +- 14 files changed, 60 insertions(+), 56 deletions(-) rename steering_controllers_library/test/{test_steering_controllers_library_ackermann.cpp => test_steering_controllers_library_ackermann_input.cpp} (96%) rename steering_controllers_library/test/{test_steering_controllers_library_ackermann.hpp => test_steering_controllers_library_ackermann_input.hpp} (94%) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index f4903a5d32..2576d078f2 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -117,7 +117,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(), params_.twist_input); + last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.use_twist_input); } else { diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index bd990ea575..493f7dda17 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -67,7 +67,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(), params_.twist_input); + last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.use_twist_input); } else { diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 2bd0b3d9de..37bc8c2732 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -66,12 +66,12 @@ if(BUILD_TESTING) hardware_interface ) add_rostest_with_parameters_gmock( - test_steering_controllers_library_ackermann test/test_steering_controllers_library_ackermann.cpp + test_steering_controllers_library_ackermann_input test/test_steering_controllers_library_ackermann_input.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_ackermann_params.yaml) - target_include_directories(test_steering_controllers_library_ackermann PRIVATE include) - target_link_libraries(test_steering_controllers_library_ackermann steering_controllers_library) + target_include_directories(test_steering_controllers_library_ackermann_input PRIVATE include) + target_link_libraries(test_steering_controllers_library_ackermann_input steering_controllers_library) ament_target_dependencies( - test_steering_controllers_library_ackermann + test_steering_controllers_library_ackermann_input controller_interface hardware_interface ) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 487cf94bb9..58cabf2b34 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -21,7 +21,7 @@ For an introduction to mobile robot kinematics and the nomenclature used here, s Execution logic of the controller ---------------------------------- -The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used. +The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used if ``twist_input == true``. If ``twist_input == false``, the controller uses `ackermann drive messages `_ where linear ``speed`` and angular ``steering_angle`` components are used. Values in other components are ignored. In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. @@ -82,9 +82,10 @@ With the following state interfaces: Subscribers ,,,,,,,,,,,, -Used when controller is not in chained mode (``in_chained_mode == false``). - +Used when controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is activated (``twist_input == true``): - ``/reference`` [`geometry_msgs/msg/TwistStamped `_] +When the controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is not activated (``twist_input == false``): +- ``/reference`` [`ackermann_msgs/msg/AckermannDriveStamped `_] Publishers ,,,,,,,,,,, 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 9f52fd8f3f..60d20cdbf2 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 @@ -28,11 +28,11 @@ #include "realtime_tools/realtime_publisher.hpp" // TODO(anyone): Replace with controller specific messages +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "control_msgs/msg/steering_controller_status.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "steering_controllers_library/steering_controllers_library_parameters.hpp" #include "steering_controllers_library/steering_odometry.hpp" 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 0751765ade..502eeaf09e 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -136,10 +136,10 @@ class SteeringOdometry * \param v_bx Linear velocity [m/s] * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call - * \param twist_input If true, the input is twist, otherwise it is steering angle + * \param use_twist_input If true, the input is twist, otherwise it is steering angle */ void update_open_loop( - const double v_bx, const double omega_bz, const double dt, const bool twist_input = true); + const double v_bx, const double omega_bz, const double dt, const bool use_twist_input = true); /** * \brief Set odometry type @@ -208,13 +208,14 @@ class SteeringOdometry * \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 reduce_wheel_speed_until_steering_reached Reduce wheel speed until the steering angle - * \param twist_input If true, the input is twist, otherwise it is steering angle + * \param use_twist_input If true, the input is twist, otherwise it is steering angle * has been reached * \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 = true, - const bool reduce_wheel_speed_until_steering_reached = false, const bool twist_input = true); + const bool reduce_wheel_speed_until_steering_reached = false, + const bool use_twist_input = true); /** * \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 75c9775c5a..c857ccf64e 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -344,7 +344,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.twist_input) + if (params_.use_twist_input) { ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, @@ -565,8 +565,8 @@ SteeringControllersLibrary::on_export_reference_interfaces() reference_interfaces.push_back( hardware_interface::CommandInterface( get_node()->get_name() + std::string("/angular"), - (params_.twist_input ? hardware_interface::HW_IF_VELOCITY - : hardware_interface::HW_IF_POSITION), + (params_.use_twist_input ? hardware_interface::HW_IF_VELOCITY + : hardware_interface::HW_IF_POSITION), &reference_interfaces_[1])); return reference_interfaces; @@ -578,7 +578,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - if (params_.twist_input) + if (params_.use_twist_input) { reset_controller_reference_msg(*(input_ref_twist_.readFromRT()), get_node()); } @@ -603,7 +603,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_deactivate( controller_interface::return_type SteeringControllersLibrary::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - if (params_.twist_input) + if (params_.use_twist_input) { auto current_ref = *(input_ref_twist_.readFromRT()); if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) @@ -638,8 +638,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { const auto age_of_last_command = - params_.twist_input ? time - (*(input_ref_twist_.readFromRT()))->header.stamp - : time - (*(input_ref_ackermann_.readFromRT()))->header.stamp; + params_.use_twist_input ? time - (*(input_ref_twist_.readFromRT()))->header.stamp + : time - (*(input_ref_ackermann_.readFromRT()))->header.stamp; const auto timeout = age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); @@ -650,7 +650,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, - params_.reduce_wheel_speed_until_steering_reached, params_.twist_input); + params_.reduce_wheel_speed_until_steering_reached, params_.use_twist_input); for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 3276dc7373..5e208802ce 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -168,9 +168,9 @@ steering_controllers_library: read_only: false, } - twist_input: { + use_twist_input: { type: bool, default_value: true, - description: "Choose whether a Twist/TwistStamped or a AckermannDriveStamped message is used as input.", - read_only: false, + description: "Choose whether a TwistStamped or a AckermannDriveStamped message is used as input.", + read_only: true, } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 52fc248ee1..bcda896eec 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -189,11 +189,12 @@ bool SteeringOdometry::update_from_velocity( } void SteeringOdometry::update_open_loop( - const double v_bx, const double omega_bz, const double dt, const bool twist_input) + const double v_bx, const double omega_bz, const double dt, const bool use_twist_input) { /// Save last linear and angular velocity: linear_ = v_bx; - angular_ = twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz); + angular_ = + use_twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz); /// Integrate odometry: integrate_fk(v_bx, omega_bz, dt); @@ -242,7 +243,7 @@ double SteeringOdometry::convert_steering_angle_to_angular_velocity(double v_bx, std::tuple, std::vector> SteeringOdometry::get_commands( const double v_bx, const double omega_bz, const bool open_loop, - const bool reduce_wheel_speed_until_steering_reached, const bool twist_input) + const bool reduce_wheel_speed_until_steering_reached, const bool use_twist_input) { // desired wheel speed and steering angle of the middle of traction and steering axis double Ws, phi, phi_IK = steer_pos_; @@ -261,7 +262,8 @@ std::tuple, std::vector> SteeringOdometry::get_comma } #endif // steering angle - phi = twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz) : omega_bz; + phi = + use_twist_input ? SteeringOdometry::convert_twist_to_steering_angle(v_bx, omega_bz) : omega_bz; if (open_loop) { phi_IK = phi; diff --git a/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml b/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml index cad7c030cf..fcf643b17b 100644 --- a/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml @@ -6,12 +6,12 @@ test_steering_controllers_library: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - twist_input: false + use_twist_input: false rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 - front_wheel_track: 2.12321 - rear_wheel_track: 1.76868 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + steering_track_width: 2.12321 + traction_track_width: 1.76868 + steering_wheels_radius: 0.45 + traction_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index 1b0247c9da..88282ff70e 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -5,12 +5,12 @@ test_steering_controllers_library: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - twist_input: true + use_twist_input: true traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint] steering_joints_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 - front_wheel_track: 2.12321 - rear_wheel_track: 1.76868 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + steering_track_width: 2.12321 + traction_track_width: 1.76868 + steering_wheels_radius: 0.45 + traction_wheels_radius: 0.45 diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann_input.cpp similarity index 96% rename from steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp rename to steering_controllers_library/test/test_steering_controllers_library_ackermann_input.cpp index eb3c6339cd..374c0b560b 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann_input.cpp @@ -18,7 +18,7 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "test_steering_controllers_library_ackermann.hpp" +#include "test_steering_controllers_library_ackermann_input.hpp" class SteeringControllersLibraryTest : public SteeringControllersLibraryFixture @@ -36,16 +36,16 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + traction_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + traction_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + steering_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], - front_wheels_names_[1] + "/" + steering_interface_name_); + steering_wheels_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp b/steering_controllers_library/test/test_steering_controllers_library_ackermann_input.hpp similarity index 94% rename from steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp rename to steering_controllers_library/test/test_steering_controllers_library_ackermann_input.hpp index 6f9d7dfdfd..8e0a07f803 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_ackermann_input.hpp @@ -173,25 +173,25 @@ class SteeringControllersLibraryFixture : public ::testing::Test command_itfs_.emplace_back( hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, + traction_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back( hardware_interface::CommandInterface( - rear_wheels_names_[1], steering_interface_name_, + traction_wheels_names_[1], steering_interface_name_, &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back( hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, + steering_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back( hardware_interface::CommandInterface( - front_wheels_names_[1], steering_interface_name_, + steering_wheels_names_[1], steering_interface_name_, &joint_command_values_[CMD_STEER_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); @@ -201,25 +201,25 @@ class SteeringControllersLibraryFixture : public ::testing::Test state_itfs_.emplace_back( hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, + traction_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back( hardware_interface::StateInterface( - rear_wheels_names_[1], traction_interface_name_, + traction_wheels_names_[1], traction_interface_name_, &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back( hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, + steering_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back( hardware_interface::StateInterface( - front_wheels_names_[1], steering_interface_name_, + steering_wheels_names_[1], steering_interface_name_, &joint_state_values_[STATE_STEER_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); @@ -303,11 +303,11 @@ class SteeringControllersLibraryFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector front_wheels_names_ = { + std::vector traction_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steering_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; std::vector joint_names_ = { - rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + traction_wheels_names_[0], traction_wheels_names_[1], steering_wheels_names_[0], steering_wheels_names_[1]}; std::vector rear_wheels_preceeding_names_ = { "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 6e3809818f..d542786fc4 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -83,7 +83,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(), params_.twist_input); + last_linear_velocity_, last_angular_velocity_, period.seconds(), params_.use_twist_input); } else { From d01dd0f0bf7bb59ee7545ffda204574df3a51a0c Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Thu, 12 Jun 2025 08:53:57 +0000 Subject: [PATCH 20/26] Merge branch 'master' of github.com:wittenator/ros2_controllers --- steering_controllers_library/CMakeLists.txt | 10 ++-- .../steering_controllers_library.hpp | 12 ++--- steering_controllers_library/package.xml | 1 - .../src/steering_controllers_library.cpp | 53 +++++++++---------- .../src/steering_controllers_library.yaml | 2 +- ...ollers_library_steering_input_params.yaml} | 7 ++- .../test_steering_controllers_library.hpp | 4 +- ...ng_controllers_library_steering_input.cpp} | 46 +++++++--------- ...ng_controllers_library_steering_input.hpp} | 38 ++++++------- 9 files changed, 80 insertions(+), 93 deletions(-) rename steering_controllers_library/test/{steering_controllers_library_ackermann_params.yaml => steering_controllers_library_steering_input_params.yaml} (60%) rename steering_controllers_library/test/{test_steering_controllers_library_ackermann_input.cpp => test_steering_controllers_library_steering_input.cpp} (80%) rename steering_controllers_library/test/{test_steering_controllers_library_ackermann_input.hpp => test_steering_controllers_library_steering_input.hpp} (90%) diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index 520691d8c6..47eeaa2308 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -21,7 +21,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2 tf2_msgs tf2_geometry_msgs - ackermann_msgs ) find_package(ament_cmake REQUIRED) @@ -56,7 +55,6 @@ target_link_libraries(steering_controllers_library PUBLIC realtime_tools::realtime_tools tf2::tf2 tf2_geometry_msgs::tf2_geometry_msgs - ${ackermann_msgs_TARGETS} ${tf2_msgs_TARGETS} ${geometry_msgs_TARGETS} ${control_msgs_TARGETS} @@ -75,10 +73,10 @@ if(BUILD_TESTING) target_link_libraries(test_steering_controllers_library steering_controllers_library) add_rostest_with_parameters_gmock( - test_steering_controllers_library_ackermann_input test/test_steering_controllers_library_ackermann_input.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_ackermann_params.yaml) - target_include_directories(test_steering_controllers_library_ackermann_input PRIVATE include) - target_link_libraries(test_steering_controllers_library_ackermann_input steering_controllers_library) + test_steering_controllers_library_steering_input test/test_steering_controllers_library_steering_input.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_steering_input_params.yaml) + target_include_directories(test_steering_controllers_library_steering_input PRIVATE include) + target_link_libraries(test_steering_controllers_library_steering_input steering_controllers_library) ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) target_link_libraries(test_steering_odometry steering_controllers_library) 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 60d20cdbf2..399ba76de3 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 @@ -28,7 +28,7 @@ #include "realtime_tools/realtime_publisher.hpp" // TODO(anyone): Replace with controller specific messages -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "control_msgs/msg/steering_controller_command.hpp" #include "control_msgs/msg/steering_controller_status.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -72,7 +72,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl const rclcpp::Time & time, const rclcpp::Duration & period) override; using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped; - using ControllerAckermannReferenceMsg = ackermann_msgs::msg::AckermannDriveStamped; + using ControllerSteeringReferenceMsg = control_msgs::msg::SteeringControllerCommand; using ControllerStateMsgOdom = nav_msgs::msg::Odometry; using ControllerStateMsgTf = tf2_msgs::msg::TFMessage; using SteeringControllerStateMsg = control_msgs::msg::SteeringControllerStatus; @@ -86,11 +86,11 @@ 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_ = + rclcpp::Subscription::SharedPtr ref_subscriber_steering_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_twist_; - realtime_tools::RealtimeBuffer> - input_ref_ackermann_; + realtime_tools::RealtimeBuffer> + input_ref_steering_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms using ControllerStatePublisherOdom = realtime_tools::RealtimePublisher; @@ -133,7 +133,7 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl private: // callback for topic interface void reference_callback_twist(const std::shared_ptr msg); - void reference_callback_ackermann(const std::shared_ptr msg); + void reference_callback_steering(const std::shared_ptr msg); }; } // namespace steering_controllers_library diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index f1721e2b97..7981c31e55 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -40,7 +40,6 @@ tf2 tf2_msgs tf2_geometry_msgs - ackermann_msgs ament_cmake_gmock controller_manager diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5425a42472..bd4d8beeca 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -28,8 +28,8 @@ namespace using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; -using ControllerAckermannReferenceMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; +using ControllerSteeringReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerSteeringReferenceMsg; // called from RT control loop void reset_controller_reference_msg( @@ -46,15 +46,12 @@ void reset_controller_reference_msg( } void reset_controller_reference_msg( - const std::shared_ptr & 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(); + msg->linear_velocity = std::numeric_limits::quiet_NaN(); + msg->steering_angle = std::numeric_limits::quiet_NaN(); } } // namespace @@ -128,7 +125,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Ackermann configuration requires exactly two traction joints, but %zu were provided", + "Steering configuration requires exactly two traction joints, but %zu were provided", params_.traction_joints_names.size()); return controller_interface::CallbackReturn::ERROR; } @@ -162,7 +159,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Ackermann configuration requires exactly two steering joints, but %zu were provided", + "Steering configuration requires exactly two steering joints, but %zu were provided", params_.steering_joints_names.size()); return controller_interface::CallbackReturn::ERROR; } @@ -203,7 +200,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Ackermann configuration requires exactly two traction joints, but %zu state interface " + "Steering configuration requires exactly two traction joints, but %zu state interface " "names were provided", params_.traction_joints_state_names.size()); return controller_interface::CallbackReturn::ERROR; @@ -248,7 +245,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Ackermann configuration requires exactly two steering joints, but %zu state interface " + "Steering configuration requires exactly two steering joints, but %zu state interface " "names were provided", params_.steering_joints_state_names.size()); return controller_interface::CallbackReturn::ERROR; @@ -282,14 +279,14 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( } else { - ref_subscriber_ackermann_ = get_node()->create_subscription( + ref_subscriber_steering_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind( - &SteeringControllersLibrary::reference_callback_ackermann, this, std::placeholders::_1)); - std::shared_ptr msg = - std::make_shared(); + &SteeringControllersLibrary::reference_callback_steering, this, std::placeholders::_1)); + std::shared_ptr msg = + std::make_shared(); reset_controller_reference_msg(msg, get_node()); - input_ref_ackermann_.writeFromNonRT(msg); + input_ref_steering_.writeFromNonRT(msg); } try @@ -400,8 +397,8 @@ void SteeringControllersLibrary::reference_callback_twist( } } -void SteeringControllersLibrary::reference_callback_ackermann( - const std::shared_ptr msg) +void SteeringControllersLibrary::reference_callback_steering( + 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) @@ -415,7 +412,7 @@ void SteeringControllersLibrary::reference_callback_ackermann( if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) { - input_ref_ackermann_.writeFromNonRT(msg); + input_ref_steering_.writeFromNonRT(msg); } else { @@ -509,7 +506,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( } else { - reset_controller_reference_msg(*(input_ref_ackermann_.readFromRT()), get_node()); + reset_controller_reference_msg(*(input_ref_steering_.readFromRT()), get_node()); } return controller_interface::CallbackReturn::SUCCESS; @@ -539,11 +536,11 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f } else { - auto current_ref = *(input_ref_ackermann_.readFromRT()); - if (!std::isnan(current_ref->drive.speed) && !std::isnan(current_ref->drive.steering_angle)) + auto current_ref = *(input_ref_steering_.readFromRT()); + if (!std::isnan(current_ref->linear_velocity) && !std::isnan(current_ref->steering_angle)) { - reference_interfaces_[0] = current_ref->drive.speed; - reference_interfaces_[1] = current_ref->drive.steering_angle; + reference_interfaces_[0] = current_ref->linear_velocity; + reference_interfaces_[1] = current_ref->steering_angle; } } @@ -562,9 +559,9 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) { - const auto age_of_last_command = - params_.use_twist_input ? time - (*(input_ref_twist_.readFromRT()))->header.stamp - : time - (*(input_ref_ackermann_.readFromRT()))->header.stamp; + const auto age_of_last_command = params_.use_twist_input + ? time - (*(input_ref_twist_.readFromRT()))->header.stamp + : time - (*(input_ref_steering_.readFromRT()))->header.stamp; const auto timeout = age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 26c4eb10f1..678d7b985d 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -118,6 +118,6 @@ steering_controllers_library: use_twist_input: { type: bool, default_value: true, - description: "Choose whether a TwistStamped or a AckermannDriveStamped message is used as input.", + description: "Choose whether a TwistStamped or a SteeringControllerCommand message is used as input.", read_only: true, } diff --git a/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml b/steering_controllers_library/test/steering_controllers_library_steering_input_params.yaml similarity index 60% rename from steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml rename to steering_controllers_library/test/steering_controllers_library_steering_input_params.yaml index fcf643b17b..7515de850f 100644 --- a/steering_controllers_library/test/steering_controllers_library_ackermann_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_steering_input_params.yaml @@ -1,14 +1,13 @@ -test_steering_controllers_library: +test_steering_controllers_steering_input_library: ros__parameters: reference_timeout: 0.1 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false use_twist_input: false - rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] - front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 steering_track_width: 2.12321 diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 0cabb075b5..0040f679ba 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -35,8 +35,8 @@ using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg; using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; -using ControllerAckermannReferenceMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; +using ControllerSteeringReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerSteeringReferenceMsg; // NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann_input.cpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp similarity index 80% rename from steering_controllers_library/test/test_steering_controllers_library_ackermann_input.cpp rename to steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp index 374c0b560b..13c5e8641b 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann_input.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp @@ -18,15 +18,15 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "test_steering_controllers_library_ackermann_input.hpp" +#include "test_steering_controllers_library_steering_input.hpp" -class SteeringControllersLibraryTest -: public SteeringControllersLibraryFixture +class SteeringControllersLibrarySteeringInputTest +: public SteeringControllersSteeringInputLibraryFixture { }; // checking if all interfaces, command, state and reference are exported as expected -TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) +TEST_F(SteeringControllersLibrarySteeringInputTest, check_exported_interfaces) { SetUpController(); @@ -82,7 +82,7 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) // Tests controller update_reference_from_subscribers and // its two cases for position_feedback true/false behavior // when too old msg is sent i.e age_of_last_command > ref_timeout case -TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) +TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for_ref_timeout) { SetUpController(); @@ -103,22 +103,19 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) const float TEST_LINEAR_VELOCITY_X = static_cast(1.5); const float TEST_STEERING_ANGLE = static_cast(0.575875); - std::shared_ptr msg = - std::make_shared(); + std::shared_ptr msg = + std::make_shared(); // adjusting to achieve age_of_last_command > ref_timeout msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); - msg->drive.speed = TEST_LINEAR_VELOCITY_X; - msg->drive.steering_angle = TEST_STEERING_ANGLE; - msg->drive.steering_angle_velocity = std::numeric_limits::quiet_NaN(); - msg->drive.acceleration = std::numeric_limits::quiet_NaN(); - msg->drive.jerk = std::numeric_limits::quiet_NaN(); - controller_->input_ref_ackermann_.writeFromNonRT(msg); + msg->linear_velocity = TEST_LINEAR_VELOCITY_X; + msg->steering_angle = TEST_STEERING_ANGLE; + controller_->input_ref_steering_.writeFromNonRT(msg); const auto age_of_last_command = controller_->get_node()->now() - - (*(controller_->input_ref_ackermann_.readFromNonRT()))->header.stamp; + (*(controller_->input_ref_steering_.readFromNonRT()))->header.stamp; // case 1 position_feedback = false controller_->params_.position_feedback = false; @@ -126,7 +123,7 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ( - (*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + (*(controller_->input_ref_steering_.readFromRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -138,9 +135,9 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } ASSERT_EQ( - (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + (*(controller_->input_ref_steering_.readFromNonRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle, + (*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle, TEST_STEERING_ANGLE); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); @@ -163,17 +160,14 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // adjusting to achieve age_of_last_command > ref_timeout msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); - msg->drive.speed = TEST_LINEAR_VELOCITY_X; - msg->drive.steering_angle = TEST_STEERING_ANGLE; - msg->drive.steering_angle_velocity = std::numeric_limits::quiet_NaN(); - msg->drive.acceleration = std::numeric_limits::quiet_NaN(); - msg->drive.jerk = std::numeric_limits::quiet_NaN(); - controller_->input_ref_ackermann_.writeFromNonRT(msg); + msg->linear_velocity = TEST_LINEAR_VELOCITY_X; + msg->steering_angle = TEST_STEERING_ANGLE; + controller_->input_ref_steering_.writeFromNonRT(msg); // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ( - (*(controller_->input_ref_ackermann_.readFromRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + (*(controller_->input_ref_steering_.readFromRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -185,9 +179,9 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } ASSERT_EQ( - (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.speed, TEST_LINEAR_VELOCITY_X); + (*(controller_->input_ref_steering_.readFromNonRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - (*(controller_->input_ref_ackermann_.readFromNonRT()))->drive.steering_angle, + (*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle, TEST_STEERING_ANGLE); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); diff --git a/steering_controllers_library/test/test_steering_controllers_library_ackermann_input.hpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp similarity index 90% rename from steering_controllers_library/test/test_steering_controllers_library_ackermann_input.hpp rename to steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp index 16df548410..5de0872784 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_ackermann_input.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_ -#define TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_ +#ifndef TEST_STEERING_CONTROLLERS_LIBRARY_STEERING_INPUT_HPP_ +#define TEST_STEERING_CONTROLLERS_LIBRARY_STEERING_INPUT_HPP_ #include @@ -33,10 +33,10 @@ using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::SteeringControllerStateMsg; -using ControllerAckermannReferenceMsg = - steering_controllers_library::SteeringControllersLibrary::ControllerAckermannReferenceMsg; +using ControllerSteeringReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerSteeringReferenceMsg; -// NOTE: Testing steering_controllers_library for Ackermann vehicle configuration only +// NOTE: Testing steering_controllers_library for Steering vehicle configuration only // name constants for state interfaces static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0; @@ -68,11 +68,11 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; // namespace // subclassing and friending so we can access member variables -class TestableSteeringControllersLibrary +class TestableSteeringControllersSteeringInputLibrary : public steering_controllers_library::SteeringControllersLibrary { - FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); - FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); + FRIEND_TEST(SteeringControllersLibrarySteeringInputTest, check_exported_interfaces); + FRIEND_TEST(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for_ref_timeout); public: controller_interface::CallbackReturn on_configure( @@ -89,7 +89,7 @@ class TestableSteeringControllersLibrary } /** - * @brief wait_for_command blocks until a new ControllerAckermannReferenceMsg is received. + * @brief wait_for_command blocks until a new ControllerSteeringReferenceMsg is received. * Requires that the executor is not spinned elsewhere between the * message publication and the call to this function. */ @@ -132,7 +132,7 @@ class TestableSteeringControllersLibrary // We are using template class here for easier reuse of Fixture in specializations of controllers template -class SteeringControllersLibraryFixture : public ::testing::Test +class SteeringControllersSteeringInputLibraryFixture : public ::testing::Test { public: static void SetUpTestCase() {} @@ -143,8 +143,8 @@ class SteeringControllersLibraryFixture : public ::testing::Test controller_ = std::make_unique(); command_publisher_node_ = std::make_shared("command_publisher"); - command_publisher_ = command_publisher_node_->create_publisher( - "/test_steering_controllers_library/reference", rclcpp::SystemDefaultsQoS()); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_steering_controllers_steering_input_library/reference", rclcpp::SystemDefaultsQoS()); } static void TearDownTestCase() {} @@ -152,7 +152,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test void TearDown() { controller_.reset(nullptr); } protected: - void SetUpController(const std::string controller_name = "test_steering_controllers_library") + void SetUpController(const std::string controller_name = "test_steering_controllers_steering_input_library") { ASSERT_EQ( controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), @@ -233,7 +233,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test rclcpp::Node test_subscription_node("test_subscription_node"); auto subs_callback = [&](const ControllerStateMsg::SharedPtr cb_msg) { received_msg = cb_msg; }; auto subscription = test_subscription_node.create_subscription( - "/test_steering_controllers_library/controller_state", 10, subs_callback); + "/test_steering_controllers_steering_input_library/controller_state", 10, subs_callback); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(test_subscription_node.get_node_base_interface()); @@ -289,9 +289,9 @@ class SteeringControllersLibraryFixture : public ::testing::Test wait_for_topic(command_publisher_->get_topic_name()); - ControllerAckermannReferenceMsg msg; - msg.drive.speed = linear; - msg.drive.steering_angle = angular; + ControllerSteeringReferenceMsg msg; + msg.linear_velocity = linear; + msg.steering_angle = angular; command_publisher_->publish(msg); } @@ -340,7 +340,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test // Test related parameters std::unique_ptr controller_; rclcpp::Node::SharedPtr command_publisher_node_; - rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Publisher::SharedPtr command_publisher_; }; -#endif // TEST_STEERING_CONTROLLERS_LIBRARY_ACKERMANN_HPP_ +#endif // TEST_STEERING_CONTROLLERS_LIBRARY_STEERING_INPUT_HPP_ From 1b7cf5752283e8d13eef9a1b8485badeae32f65a Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Thu, 12 Jun 2025 08:55:10 +0000 Subject: [PATCH 21/26] Fix formatting --- .../test_steering_controllers_library_steering_input.cpp | 9 ++++----- .../test_steering_controllers_library_steering_input.hpp | 6 ++++-- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp index 13c5e8641b..a861ffa84f 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp @@ -21,7 +21,8 @@ #include "test_steering_controllers_library_steering_input.hpp" class SteeringControllersLibrarySteeringInputTest -: public SteeringControllersSteeringInputLibraryFixture +: public SteeringControllersSteeringInputLibraryFixture< + TestableSteeringControllersSteeringInputLibrary> { }; @@ -137,8 +138,7 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for ASSERT_EQ( (*(controller_->input_ref_steering_.readFromNonRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - (*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle, - TEST_STEERING_ANGLE); + (*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle, TEST_STEERING_ANGLE); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -181,8 +181,7 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for ASSERT_EQ( (*(controller_->input_ref_steering_.readFromNonRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( - (*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle, - TEST_STEERING_ANGLE); + (*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle, TEST_STEERING_ANGLE); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp index 5de0872784..4d59c2dd57 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp @@ -72,7 +72,8 @@ class TestableSteeringControllersSteeringInputLibrary : public steering_controllers_library::SteeringControllersLibrary { FRIEND_TEST(SteeringControllersLibrarySteeringInputTest, check_exported_interfaces); - FRIEND_TEST(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for_ref_timeout); + FRIEND_TEST( + SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for_ref_timeout); public: controller_interface::CallbackReturn on_configure( @@ -152,7 +153,8 @@ class SteeringControllersSteeringInputLibraryFixture : public ::testing::Test void TearDown() { controller_.reset(nullptr); } protected: - void SetUpController(const std::string controller_name = "test_steering_controllers_steering_input_library") + void SetUpController( + const std::string controller_name = "test_steering_controllers_steering_input_library") { ASSERT_EQ( controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), From ef29f5f26e294375e3c1f3db19484cd22b91f8a4 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Thu, 12 Jun 2025 09:36:30 +0000 Subject: [PATCH 22/26] Added consistent naming in tests and fixed docs --- steering_controllers_library/doc/userdoc.rst | 5 +-- ...ing_controllers_library_steering_input.cpp | 8 ++--- ...ing_controllers_library_steering_input.hpp | 31 +++++++++---------- 3 files changed, 21 insertions(+), 23 deletions(-) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 3cd7681de2..fd8c2bb445 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -6,6 +6,7 @@ steering_controllers_library ============================= .. _steering_controller_status_msg: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/SteeringControllerStatus.msg +.. _steering_controller_command_msg: https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/SteeringControllerCommand.msg .. _odometry_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/nav_msgs/msg/Odometry.msg .. _twist_msg: https://github.com/ros2/common_interfaces/blob/{DISTRO}/geometry_msgs/msg/TwistStamped.msg .. _tf_msg: https://github.com/ros2/geometry2/blob/{DISTRO}/tf2_msgs/msg/TFMessage.msg @@ -21,7 +22,7 @@ For an introduction to mobile robot kinematics and the nomenclature used here, s Execution logic of the controller ---------------------------------- -The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used if ``twist_input == true``. If ``twist_input == false``, the controller uses `ackermann drive messages `_ where linear ``speed`` and angular ``steering_angle`` components are used. +The controller uses velocity input, i.e., stamped `twist messages `_ where linear ``x`` and angular ``z`` components are used if ``twist_input == true``. If ``twist_input == false``, the controller uses `control_msgs/msg/SteeringControllerStatus `_ where linear ``speed`` and angular ``steering_angle`` components are used. Values in other components are ignored. In the chain mode the controller provides two reference interfaces, one for linear velocity and one for steering angle position. @@ -85,7 +86,7 @@ Subscribers Used when controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is activated (``twist_input == true``): - ``/reference`` [`geometry_msgs/msg/TwistStamped `_] When the controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is not activated (``twist_input == false``): -- ``/reference`` [`ackermann_msgs/msg/AckermannDriveStamped `_] +- ``/reference`` [`control_msgs/msg/SteeringControllerCommand `_] Publishers ,,,,,,,,,,, diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp index a861ffa84f..fab4689792 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp @@ -37,16 +37,16 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], - traction_wheels_names_[0] + "/" + traction_interface_name_); + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - traction_wheels_names_[1] + "/" + traction_interface_name_); + traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], - steering_wheels_names_[0] + "/" + steering_interface_name_); + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], - steering_wheels_names_[1] + "/" + steering_interface_name_); + steering_joints_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp index 4d59c2dd57..706494dadf 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp @@ -175,25 +175,25 @@ class SteeringControllersSteeringInputLibraryFixture : public ::testing::Test command_itfs_.emplace_back( hardware_interface::CommandInterface( - traction_wheels_names_[0], traction_interface_name_, + traction_joints_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back( hardware_interface::CommandInterface( - traction_wheels_names_[1], steering_interface_name_, + traction_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back( hardware_interface::CommandInterface( - steering_wheels_names_[0], steering_interface_name_, + steering_joints_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back( hardware_interface::CommandInterface( - steering_wheels_names_[1], steering_interface_name_, + steering_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_STEER_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); @@ -203,25 +203,25 @@ class SteeringControllersSteeringInputLibraryFixture : public ::testing::Test state_itfs_.emplace_back( hardware_interface::StateInterface( - traction_wheels_names_[0], traction_interface_name_, + traction_joints_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back( hardware_interface::StateInterface( - traction_wheels_names_[1], traction_interface_name_, + traction_joints_names_[1], traction_interface_name_, &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back( hardware_interface::StateInterface( - steering_wheels_names_[0], steering_interface_name_, + steering_joints_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back( hardware_interface::StateInterface( - steering_wheels_names_[1], steering_interface_name_, + steering_joints_names_[1], steering_interface_name_, &joint_state_values_[STATE_STEER_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); @@ -305,21 +305,18 @@ class SteeringControllersSteeringInputLibraryFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector traction_wheels_names_ = { + std::vector traction_joints_names_ = { "rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector steering_wheels_names_ = { + std::vector steering_joints_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; std::vector joint_names_ = { - traction_wheels_names_[0], traction_wheels_names_[1], steering_wheels_names_[0], - steering_wheels_names_[1]}; + traction_joints_names_[0], traction_joints_names_[1], steering_joints_names_[0], + steering_joints_names_[1]}; - std::vector rear_wheels_preceeding_names_ = { + std::vector traction_joints_preceding_names_ = { "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = { + std::vector steering_joints_preceding_names_ = { "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; - std::vector preceeding_joint_names_ = { - rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], - front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; double wheelbase_ = 3.24644; double front_wheel_track_ = 2.12321; From 39dafe91122b074afeee83aa8bd11ec67d001732 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Mon, 11 Aug 2025 12:35:46 +0000 Subject: [PATCH 23/26] fix tests --- ...ing_controllers_library_steering_input.cpp | 48 +++++++++++++------ 1 file changed, 33 insertions(+), 15 deletions(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp index fab4689792..876407231c 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp @@ -107,16 +107,27 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for std::shared_ptr msg = std::make_shared(); - // adjusting to achieve age_of_last_command > ref_timeout + // First send a valid (non-timeout) message to establish command interface values + msg->header.stamp = controller_->get_node()->now(); + msg->linear_velocity = TEST_LINEAR_VELOCITY_X; + msg->steering_angle = TEST_STEERING_ANGLE; + controller_->input_ref_steering_.set(*msg); + + // Process the valid message + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // Now send the timeout message to test timeout behavior msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); msg->linear_velocity = TEST_LINEAR_VELOCITY_X; msg->steering_angle = TEST_STEERING_ANGLE; - controller_->input_ref_steering_.writeFromNonRT(msg); + controller_->input_ref_steering_.set(*msg); const auto age_of_last_command = controller_->get_node()->now() - - (*(controller_->input_ref_steering_.readFromNonRT()))->header.stamp; + controller_->input_ref_steering_.get().header.stamp; // case 1 position_feedback = false controller_->params_.position_feedback = false; @@ -124,7 +135,7 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ( - (*(controller_->input_ref_steering_.readFromRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); + controller_->input_ref_steering_.get().linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -135,10 +146,8 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ( - (*(controller_->input_ref_steering_.readFromNonRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - (*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle, TEST_STEERING_ANGLE); + ASSERT_TRUE(std::isnan(controller_->input_ref_steering_.get().linear_velocity)); + ASSERT_TRUE(std::isnan(controller_->input_ref_steering_.get().steering_angle)); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -157,17 +166,28 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for // case 2 position_feedback = true controller_->params_.position_feedback = true; - // adjusting to achieve age_of_last_command > ref_timeout + // First send a valid message for the position_feedback=true case + msg->header.stamp = controller_->get_node()->now(); + msg->linear_velocity = TEST_LINEAR_VELOCITY_X; + msg->steering_angle = TEST_STEERING_ANGLE; + controller_->input_ref_steering_.set(*msg); + + // Process the valid message + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // Now send the timeout message to test timeout behavior with position_feedback=true msg->header.stamp = controller_->get_node()->now() - controller_->ref_timeout_ - rclcpp::Duration::from_seconds(0.1); msg->linear_velocity = TEST_LINEAR_VELOCITY_X; msg->steering_angle = TEST_STEERING_ANGLE; - controller_->input_ref_steering_.writeFromNonRT(msg); + controller_->input_ref_steering_.set(*msg); // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); ASSERT_EQ( - (*(controller_->input_ref_steering_.readFromRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); + controller_->input_ref_steering_.get().linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -178,10 +198,8 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for { EXPECT_TRUE(std::isnan(interface)); } - ASSERT_EQ( - (*(controller_->input_ref_steering_.readFromNonRT()))->linear_velocity, TEST_LINEAR_VELOCITY_X); - ASSERT_EQ( - (*(controller_->input_ref_steering_.readFromNonRT()))->steering_angle, TEST_STEERING_ANGLE); + ASSERT_TRUE(std::isnan(controller_->input_ref_steering_.get().linear_velocity)); + ASSERT_TRUE(std::isnan(controller_->input_ref_steering_.get().steering_angle)); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) From ed7a7828bb51dd18b95cd9b222bbaf5e65d06d1d Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Mon, 11 Aug 2025 13:56:04 +0000 Subject: [PATCH 24/26] Format with precommit --- .../src/steering_controllers_library.cpp | 16 ++++++++++++---- .../test/test_steering_controllers_library.cpp | 6 ++++-- ...eering_controllers_library_steering_input.cpp | 13 +++++-------- 3 files changed, 21 insertions(+), 14 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index e60e800e51..210c0a0063 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -552,7 +552,9 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f // accept 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_.twist.linear.x) && !std::isnan(current_ref_twist_.twist.angular.z)) + if ( + !std::isnan(current_ref_twist_.twist.linear.x) && + !std::isnan(current_ref_twist_.twist.angular.z)) { reference_interfaces_[0] = current_ref_twist_.twist.linear.x; reference_interfaces_[1] = current_ref_twist_.twist.angular.z; @@ -568,7 +570,9 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f } else { - if (!std::isnan(current_ref_twist_.twist.linear.x) && !std::isnan(current_ref_twist_.twist.angular.z)) + if ( + !std::isnan(current_ref_twist_.twist.linear.x) && + !std::isnan(current_ref_twist_.twist.angular.z)) { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); @@ -593,7 +597,9 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f // accept 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_steering_.linear_velocity) && !std::isnan(current_ref_steering_.steering_angle)) + if ( + !std::isnan(current_ref_steering_.linear_velocity) && + !std::isnan(current_ref_steering_.steering_angle)) { reference_interfaces_[0] = current_ref_steering_.linear_velocity; reference_interfaces_[1] = current_ref_steering_.steering_angle; @@ -609,7 +615,9 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f } else { - if (!std::isnan(current_ref_steering_.linear_velocity) && !std::isnan(current_ref_steering_.steering_angle)) + if ( + !std::isnan(current_ref_steering_.linear_velocity) && + !std::isnan(current_ref_steering_.steering_angle)) { reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index cb143953db..117fb58c07 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -149,7 +149,8 @@ TEST_F(SteeringControllersLibraryTest, test_position_feedback_ref_timeout) msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_twist_.set(msg); - age_of_last_command = controller_->get_node()->now() - controller_->input_ref_twist_.get().header.stamp; + age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_twist_.get().header.stamp; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); @@ -244,7 +245,8 @@ TEST_F(SteeringControllersLibraryTest, test_velocity_feedback_ref_timeout) msg.twist.angular.z = TEST_ANGULAR_VELOCITY_Z; controller_->input_ref_twist_.set(msg); - age_of_last_command = controller_->get_node()->now() - controller_->input_ref_twist_.get().header.stamp; + age_of_last_command = + controller_->get_node()->now() - controller_->input_ref_twist_.get().header.stamp; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp index 876407231c..87d2454bcd 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp @@ -112,7 +112,7 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for msg->linear_velocity = TEST_LINEAR_VELOCITY_X; msg->steering_angle = TEST_STEERING_ANGLE; controller_->input_ref_steering_.set(*msg); - + // Process the valid message ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -126,16 +126,14 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for controller_->input_ref_steering_.set(*msg); const auto age_of_last_command = - controller_->get_node()->now() - - controller_->input_ref_steering_.get().header.stamp; + controller_->get_node()->now() - controller_->input_ref_steering_.get().header.stamp; // case 1 position_feedback = false controller_->params_.position_feedback = false; // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ( - controller_->input_ref_steering_.get().linear_velocity, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(controller_->input_ref_steering_.get().linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); @@ -171,7 +169,7 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for msg->linear_velocity = TEST_LINEAR_VELOCITY_X; msg->steering_angle = TEST_STEERING_ANGLE; controller_->input_ref_steering_.set(*msg); - + // Process the valid message ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), @@ -186,8 +184,7 @@ TEST_F(SteeringControllersLibrarySteeringInputTest, test_both_update_methods_for // age_of_last_command > ref_timeout_ ASSERT_FALSE(age_of_last_command <= controller_->ref_timeout_); - ASSERT_EQ( - controller_->input_ref_steering_.get().linear_velocity, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ(controller_->input_ref_steering_.get().linear_velocity, TEST_LINEAR_VELOCITY_X); ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); From e0a58db9bff4d1dbdfd4624fcae3dee20b9dc644 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sat, 18 Oct 2025 11:04:51 +0200 Subject: [PATCH 25/26] Revert naming in doc string MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../src/steering_controllers_library.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index a98b540db1..ae6a01f224 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -133,7 +133,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Steering configuration requires exactly two traction joints, but %zu were provided", + "Ackermann configuration requires exactly two traction joints, but %zu were provided", params_.traction_joints_names.size()); return controller_interface::CallbackReturn::ERROR; } @@ -167,7 +167,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Steering configuration requires exactly two steering joints, but %zu were provided", + "Ackermann configuration requires exactly two steering joints, but %zu were provided", params_.steering_joints_names.size()); return controller_interface::CallbackReturn::ERROR; } @@ -208,7 +208,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Steering configuration requires exactly two traction joints, but %zu state interface " + "Ackermann configuration requires exactly two traction joints, but %zu state interface " "names were provided", params_.traction_joints_state_names.size()); return controller_interface::CallbackReturn::ERROR; @@ -253,7 +253,7 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "Steering configuration requires exactly two steering joints, but %zu state interface " + "Ackermann configuration requires exactly two steering joints, but %zu state interface " "names were provided", params_.steering_joints_state_names.size()); return controller_interface::CallbackReturn::ERROR; From 48a66ccdcca4a373810c1f3791db3ad27cab5954 Mon Sep 17 00:00:00 2001 From: wittenator <9154515+wittenator@users.noreply.github.com> Date: Sat, 18 Oct 2025 10:08:50 +0000 Subject: [PATCH 26/26] Integrated more feedback --- steering_controllers_library/doc/userdoc.rst | 6 ++++++ .../src/steering_controllers_library.cpp | 13 ------------- .../src/steering_odometry.cpp | 13 ++++++++----- ..._steering_controllers_library_steering_input.cpp | 2 +- ..._steering_controllers_library_steering_input.hpp | 3 +-- 5 files changed, 16 insertions(+), 21 deletions(-) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index fd8c2bb445..16f84cded4 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -85,8 +85,14 @@ Subscribers Used when controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is activated (``twist_input == true``): - ``/reference`` [`geometry_msgs/msg/TwistStamped `_] +In this configuration the controller uses : +- **Linear Velocity (`linear`)**: Represents the linear speed of the robot (in meters per second, m/s). +- **Angular Velocity (`angular`)**: Represents the angular speed of the robot (in meters per second, m/s). When the controller is not in chained mode (``in_chained_mode == false``) and the twist input mode is not activated (``twist_input == false``): - ``/reference`` [`control_msgs/msg/SteeringControllerCommand `_] +In this configuration the controller uses : +- **Linear Velocity (`speed`)**: Represents the linear velocity of the robot (in meters per second, m/s). +- **Steering angle (`steering_angle`)**: Represents the angle of the imaginary, central steering wheel relative to the vehicle’s longitudinal axis. Specific angles for individual steering joints are computed internally based on the kinematic model of the robot. (in radians, rad) Publishers ,,,,,,,,,,, diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index a98b540db1..754b7762e1 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -44,15 +44,6 @@ void reset_controller_reference_msg( msg.twist.angular.z = std::numeric_limits::quiet_NaN(); } -void reset_controller_reference_msg( - const std::shared_ptr & msg, - const std::shared_ptr & node) -{ - msg->header.stamp = node->now(); - msg->linear_velocity = std::numeric_limits::quiet_NaN(); - msg->steering_angle = std::numeric_limits::quiet_NaN(); -} - void reset_controller_reference_msg( ControllerSteeringReferenceMsg & msg, const std::shared_ptr & node) @@ -647,10 +638,6 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c const auto timeout = age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); - // store (for open loop odometry) and set commands - last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0]; - last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1]; - auto [traction_commands, steering_commands] = odometry_.get_commands( reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached, params_.use_twist_input); diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 8d223e18da..92b49fa2e2 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -182,21 +182,24 @@ bool SteeringOdometry::update_from_velocity( 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 / wheel_base_; + const double angular_velocity = + convert_steering_angle_to_angular_velocity(linear_velocity, steer_pos_); return update_odometry(linear_velocity, angular_velocity, dt); } void SteeringOdometry::update_open_loop( - const double v_bx, const double omega_bz, const double dt, const bool use_twist_input) + const double v_bx, const double last_angular_command_, const double dt, + const bool use_twist_input) { /// Save last linear and angular velocity: linear_ = v_bx; - angular_ = - use_twist_input ? omega_bz : convert_steering_angle_to_angular_velocity(v_bx, omega_bz); + angular_ = use_twist_input + ? last_angular_command_ + : convert_steering_angle_to_angular_velocity(v_bx, last_angular_command_); /// Integrate odometry: - integrate_fk(v_bx, omega_bz, dt); + integrate_fk(v_bx, last_angular_command_, dt); } void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track) diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp index 87d2454bcd..077d60f721 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2023, FaSTTUBe - Formula Student Team TU Berlin // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp index 706494dadf..9e535c1021 100644 --- a/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library_steering_input.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, FaSTTUBe - Formula Student Team TU Berlin // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -331,7 +331,6 @@ class SteeringControllersSteeringInputLibraryFixture : public ::testing::Test std::string steering_interface_name_ = "position"; // defined in setup std::string traction_interface_name_ = ""; - std::string preceeding_prefix_ = "pid_controller"; std::vector state_itfs_; std::vector command_itfs_;