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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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/22] 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;