diff --git a/steering_controllers_library/CMakeLists.txt b/steering_controllers_library/CMakeLists.txt index ae27c5d645..22ba8611ce 100644 --- a/steering_controllers_library/CMakeLists.txt +++ b/steering_controllers_library/CMakeLists.txt @@ -68,6 +68,17 @@ if(BUILD_TESTING) ament_add_gmock(test_steering_odometry test/test_steering_odometry.cpp) target_link_libraries(test_steering_odometry steering_controllers_library) + add_rostest_with_parameters_gmock( + test_steering_controllers_open_library test/test_steering_controllers_library.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/steering_controllers_library_open_loop_timeout_params.yaml) + target_include_directories(test_steering_controllers_open_library PRIVATE include) + target_link_libraries(test_steering_controllers_open_library steering_controllers_library) + ament_target_dependencies( + test_steering_controllers_open_library + controller_interface + hardware_interface + ) + endif() install( diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 37999b6b36..3c554ead5a 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -341,6 +341,15 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_activate( // Set default value in command reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + // Set the initial steering angle values + size_t steering_count_idx = + params_.front_steering ? params_.front_wheels_names.size() : params_.rear_wheels_names.size(); + + for (size_t i = 0; i < steering_count_idx; i++) + { + command_interfaces_[steering_count_idx + i].set_value(0.575875); + } + return controller_interface::CallbackReturn::SUCCESS; } @@ -388,32 +397,52 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_linear_velocity_ = timeout ? 0.0 : reference_interfaces_[0]; last_angular_velocity_ = timeout ? 0.0 : reference_interfaces_[1]; - auto [traction_commands, steering_commands] = odometry_.get_commands( - reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, - params_.reduce_wheel_speed_until_steering_reached); - - if (params_.front_steering) + // calculate new commands if not in timeout + if (!timeout) { - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + auto [traction_commands, steering_commands] = odometry_.get_commands( + reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, + params_.reduce_wheel_speed_until_steering_reached); + + if (params_.front_steering) { - command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); + } } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + else { - command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + { + command_interfaces_[i].set_value(traction_commands[i]); + } + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + { + command_interfaces_[i + params_.front_wheels_names.size()].set_value( + steering_commands[i]); + } } } else { + // In timeout, only reset wheel velocities to zero + if (params_.front_steering) { - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) + for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i].set_value(timeout ? 0.0 : traction_commands[i]); + command_interfaces_[i].set_value(0.0); } - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) + } + else + { + for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i + params_.front_wheels_names.size()].set_value( - steering_commands[i]); + command_interfaces_[i].set_value(0.0); } } } diff --git a/steering_controllers_library/test/steering_controllers_library_open_loop_timeout_params.yaml b/steering_controllers_library/test/steering_controllers_library_open_loop_timeout_params.yaml new file mode 100644 index 0000000000..61436ec184 --- /dev/null +++ b/steering_controllers_library/test/steering_controllers_library_open_loop_timeout_params.yaml @@ -0,0 +1,17 @@ +test_steering_controllers_library: + ros__parameters: + + reference_timeout: 0.1 + front_steering: true + open_loop: true + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + 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/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 10a0f00376..a7dd0024a1 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -123,12 +123,35 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // case 1 position_feedback = false controller_->params_.position_feedback = false; + // pre odometry values + auto pre_odom_x_1 = controller_->odometry_.get_x(); + auto pre_odom_y_1 = controller_->odometry_.get_y(); + auto pre_odom_heading_1 = controller_->odometry_.get_heading(); + // 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_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // post odometry values + auto post_odom_x_1 = controller_->odometry_.get_x(); + auto post_odom_y_1 = controller_->odometry_.get_y(); + auto post_odom_heading_1 = controller_->odometry_.get_heading(); + + const double position_tolerance = 1e-5; + + // Position should remain stable + EXPECT_NEAR(pre_odom_x_1, post_odom_x_1, position_tolerance); + EXPECT_NEAR(pre_odom_y_1, post_odom_y_1, position_tolerance); + EXPECT_NEAR(pre_odom_heading_1, post_odom_heading_1, position_tolerance); + + EXPECT_DOUBLE_EQ(controller_->last_linear_velocity_, 0.0); + EXPECT_DOUBLE_EQ(controller_->last_angular_velocity_, 0.0); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); @@ -153,9 +176,18 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); + // Linear and angular velocity should be reset to zero + EXPECT_DOUBLE_EQ(controller_->odometry_.get_linear(), 0.0); + EXPECT_DOUBLE_EQ(controller_->odometry_.get_angular(), 0.0); + // case 2 position_feedback = true controller_->params_.position_feedback = true; + // pre odometry values + auto pre_odom_x_2 = controller_->odometry_.get_x(); + auto pre_odom_y_2 = controller_->odometry_.get_y(); + auto pre_odom_heading_2 = controller_->odometry_.get_heading(); + // 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); @@ -173,6 +205,22 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) ASSERT_EQ( controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); + ASSERT_EQ( + controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // post odometry values + auto post_odom_x_2 = controller_->odometry_.get_x(); + auto post_odom_y_2 = controller_->odometry_.get_y(); + auto post_odom_heading_2 = controller_->odometry_.get_heading(); + + // Position should remain stable + EXPECT_NEAR(pre_odom_x_2, post_odom_x_2, position_tolerance); + EXPECT_NEAR(pre_odom_y_2, post_odom_y_2, position_tolerance); + EXPECT_NEAR(pre_odom_heading_2, post_odom_heading_2, position_tolerance); + + EXPECT_DOUBLE_EQ(controller_->last_linear_velocity_, 0.0); + EXPECT_DOUBLE_EQ(controller_->last_angular_velocity_, 0.0); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); @@ -196,6 +244,10 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) // 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); + + // Linear and angular velocity should be reset to zero + EXPECT_DOUBLE_EQ(controller_->odometry_.get_linear(), 0.0); + EXPECT_DOUBLE_EQ(controller_->odometry_.get_angular(), 0.0); } int main(int argc, char ** argv) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 38d029d32b..d45a39a95e 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -55,10 +55,8 @@ 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 { @@ -127,7 +125,14 @@ class TestableSteeringControllersLibrary return controller_interface::CallbackReturn::SUCCESS; } - bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } + bool update_odometry(const rclcpp::Duration & period) + { + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + return true; + } }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -297,39 +302,18 @@ class SteeringControllersLibraryFixture : public ::testing::Test 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_command_values_ = {{1.1, 3.3, 0.575875, 0.575875}}; std::array joint_reference_interfaces_ = {{"linear", "angular"}}; 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_;