From afb652cb9b68adbf588be795c78b494a6ec2411e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Wed, 31 Jan 2024 21:40:02 +0100 Subject: [PATCH 1/3] Add tests for `interface_configuration_type` consistently (#899) (cherry picked from commit f2f36c0c65607659cb5961f1354a64b3d1a6866c) # Conflicts: # pid_controller/test/test_pid_controller.cpp # pid_controller/test/test_pid_controller_preceding.cpp --- .../test_ackermann_steering_controller.cpp | 28 +- .../test_ackermann_steering_controller.hpp | 2 +- ...kermann_steering_controller_preceeding.cpp | 28 +- .../test/test_admittance_controller.cpp | 3 + .../test/test_bicycle_steering_controller.cpp | 22 +- .../test/test_bicycle_steering_controller.hpp | 2 +- ...bicycle_steering_controller_preceeding.cpp | 22 +- .../test/test_diff_drive_controller.cpp | 12 +- .../test_force_torque_sensor_broadcaster.cpp | 6 + .../test/test_forward_command_controller.cpp | 9 + ...i_interface_forward_command_controller.cpp | 2 + .../test/test_imu_sensor_broadcaster.cpp | 6 + .../test/test_joint_state_broadcaster.cpp | 18 + .../test/test_trajectory_controller.cpp | 12 +- pid_controller/test/test_pid_controller.cpp | 532 ++++++++++++++++++ .../test/test_pid_controller_preceding.cpp | 104 ++++ .../test/test_range_sensor_broadcaster.cpp | 4 + .../test_steering_controllers_library.cpp | 28 +- .../test_steering_controllers_library.hpp | 2 +- .../test_tricycle_steering_controller.cpp | 37 +- .../test_tricycle_steering_controller.hpp | 2 +- ...ricycle_steering_controller_preceeding.cpp | 34 +- 22 files changed, 804 insertions(+), 111 deletions(-) create mode 100644 pid_controller/test/test_pid_controller.cpp create mode 100644 pid_controller/test/test_pid_controller_preceding.cpp diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index 480e90e166..ef5454a16c 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -46,41 +46,43 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } -TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + 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_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + 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 itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 80258258c2..0352a79cab 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -62,7 +62,7 @@ class TestableAckermannSteeringController : public ackermann_steering_controller::AckermannSteeringController { FRIEND_TEST(AckermannSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(AckermannSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(AckermannSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(AckermannSteeringControllerTest, activate_success); FRIEND_TEST(AckermannSteeringControllerTest, update_success); FRIEND_TEST(AckermannSteeringControllerTest, deactivate_success); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 2d951588c5..1a16bed838 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -48,41 +48,43 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); } -TEST_F(AckermannSteeringControllerTest, check_exported_intefaces) +TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + 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(); diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index fe1d3214e0..6b03249df8 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -157,12 +157,15 @@ TEST_F(AdmittanceControllerTest, check_interfaces) auto command_interfaces = controller_->command_interface_configuration(); ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); auto state_interfaces = controller_->state_interface_configuration(); ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->state_interfaces_.size(), diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 06b0c7e846..3dcdc0b1db 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -44,29 +44,29 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } -TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_WHEEL], + state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME auto reference_interfaces = controller_->export_reference_interfaces(); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 065f9e1a0d..b82509440b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -60,7 +60,7 @@ class TestableBicycleSteeringController : public bicycle_steering_controller::BicycleSteeringController { FRIEND_TEST(BicycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(BicycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(BicycleSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(BicycleSteeringControllerTest, activate_success); FRIEND_TEST(BicycleSteeringControllerTest, update_success); FRIEND_TEST(BicycleSteeringControllerTest, deactivate_success); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 875910ba23..bc3a182753 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -46,31 +46,31 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); } -TEST_F(BicycleSteeringControllerTest, check_exported_intefaces) +TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_WHEEL], + cmd_if_conf.names[CMD_TRACTION_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); - EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_STEER_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); - + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_WHEEL], + state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + 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(); diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index edaaa84e4b..db89d4f873 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -247,12 +247,12 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); - ASSERT_THAT( - controller_->state_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); - ASSERT_THAT( - controller_->command_interface_configuration().names, - SizeIs(left_wheel_names.size() + right_wheel_names.size())); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace) diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index b98bc8c560..b9e634ff55 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -162,8 +162,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success) // check interface configuration auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success) @@ -196,8 +198,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) // check interface configuration auto cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // deactivate passed ASSERT_EQ(fts_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -205,8 +209,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_ActivateDeactivate_Success) // check interface configuration cmd_if_conf = fts_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = fts_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(6lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Update_Success) diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index 338c569ec6..bdaa143128 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -137,6 +137,7 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(2lu)); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails) @@ -189,6 +190,7 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); ASSERT_EQ( controller_->on_activate(rclcpp_lifecycle::State()), @@ -197,8 +199,10 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); } TEST_F(ForwardCommandControllerTest, CommandSuccessTest) @@ -352,6 +356,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); node_state = controller_->get_node()->activate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); @@ -359,8 +364,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); auto command_msg = std::make_shared(); command_msg->data = {10.0, 20.0, 30.0}; @@ -383,8 +390,10 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) // check interface configuration cmd_if_conf = controller_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, SizeIs(joint_names_.size())); // did not change + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index 4942deb0bf..d50aa587c4 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -297,6 +297,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // send command auto command_ptr = std::make_shared(); @@ -322,6 +323,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state_if_conf = controller_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE); // command ptr should be reset (nullptr) after deactivation - same check as in `update` ASSERT_FALSE( diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp index f336ece2ad..0886748293 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp @@ -120,8 +120,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Configure_Success) // check interface configuration auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) @@ -139,8 +141,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // check interface configuration auto cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // deactivate passed ASSERT_EQ(imu_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -148,8 +152,10 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Activate_Success) // check interface configuration cmd_if_conf = imu_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = imu_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(10lu)); // did not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(IMUSensorBroadcasterTest, SensorName_Update_Success) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index cfe985b638..04bf10089a 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -180,8 +180,10 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -227,8 +229,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -274,8 +278,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -321,8 +327,10 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -360,9 +368,11 @@ TEST_F(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface) // check interface configuration cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT( state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); // does not change + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) @@ -381,8 +391,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -455,8 +467,10 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // publishers initialized ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); @@ -504,8 +518,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; @@ -547,8 +563,10 @@ TEST_F(JointStateBroadcasterTest, TestCustomInterfaceMapping) // check interface configuration auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = state_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // joint state initialized const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 119fb58b4d..6d0057c8f8 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -120,13 +120,13 @@ TEST_P(TrajectoryControllerTestParameterized, activate) auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - auto cmd_interface_config = traj_controller_->command_interface_configuration(); - ASSERT_EQ( - cmd_interface_config.names.size(), joint_names_.size() * command_interface_types_.size()); + auto cmd_if_conf = traj_controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_names_.size() * command_interface_types_.size()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_interface_config = traj_controller_->state_interface_configuration(); - ASSERT_EQ( - state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size()); + auto state_if_conf = traj_controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_names_.size() * state_interface_types_.size()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); state = ActivateTrajectoryController(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp new file mode 100644 index 0000000000..a44347f5f1 --- /dev/null +++ b/pid_controller/test/test_pid_controller.cpp @@ -0,0 +1,532 @@ +// 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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); + ASSERT_FALSE(controller_->params_.use_external_measured_states); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_THAT(controller_->reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); + for (const auto & dof_name : dof_names_) + { + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 10.0); + ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0); + ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); + } + ASSERT_EQ(controller_->params_.command_interface, command_interface_); + EXPECT_THAT( + controller_->params_.reference_and_state_interfaces, + testing::ElementsAreArray(state_interfaces_)); + ASSERT_FALSE(controller_->params_.use_external_measured_states); +} + +TEST_F(PidControllerTest, 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(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) + { + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); + } + 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(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +TEST_F(PidControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_EQ((*msg)->values.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values) + { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); + for (const auto & cmd : (*msg)->values_dot) + { + EXPECT_TRUE(std::isnan(cmd)); + } + + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(PidControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(PidControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PidControllerTest, test_feedforward_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to OFF + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_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()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_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()); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); + + std::shared_ptr msg = std::make_shared(); + msg->dof_names = dof_names_; + msg->values.resize(dof_names_.size(), 0.0); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + msg->values[i] = dof_command_values_[i]; + } + msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); + controller_->input_ref_.writeFromNonRT(msg); + + controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_TRUE(controller_->is_in_chained_mode()); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); + EXPECT_EQ( + controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); + EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); + for (size_t i = 0; i < dof_command_values_.size(); ++i) + { + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); + EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + } +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are not wrapped +} + +/** + * @brief check default calculation with angle_wraparound turned off + */ +TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + // write reference interface so that the values are would be wrapped + + // run update + + // check the result of the commands - the values are wrapped +} + +TEST_F(PidControllerTest, subscribe_and_get_messages_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +TEST_F(PidControllerTest, receive_message_and_publish_updated_status) +{ + 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); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); + ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + } + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + { + ASSERT_EQ(controller_->reference_interfaces_[i], 0.45); + } + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); + for (size_t i = 0; i < dof_names_.size(); ++i) + { + ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); + ASSERT_EQ(msg.dof_states[i].reference, 0.45); + ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); + } +} + +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/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp new file mode 100644 index 0000000000..3e17e69286 --- /dev/null +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -0,0 +1,104 @@ +// 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. +// +// Authors: Daniel Azanov, Dr. Denis +// + +#include "test_pid_controller.hpp" + +#include +#include +#include +#include +#include + +using pid_controller::feedforward_mode_type; + +class PidControllerTest : public PidControllerFixture +{ +}; + +TEST_F(PidControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.dof_names.empty()); + ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); + ASSERT_TRUE(controller_->params_.command_interface.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); + ASSERT_THAT( + controller_->params_.reference_and_state_dof_names, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_THAT( + controller_->reference_and_state_dof_names_, + testing::ElementsAreArray(reference_and_state_dof_names_)); + ASSERT_EQ(controller_->params_.command_interface, command_interface_); +} + +TEST_F(PidControllerTest, 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(), dof_command_values_.size()); + for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) + { + EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); + } + 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(), dof_state_values_.size()); + size_t si_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); + ++si_index; + } + } + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); + size_t ri_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + ++ri_index; + } + } +} + +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/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index e44fec4b0c..ed2aeadab1 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -154,8 +154,10 @@ TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) // check interface configuration auto cmd_if_conf = range_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); auto state_if_conf = range_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( range_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), @@ -164,8 +166,10 @@ TEST_F(RangeSensorBroadcasterTest, ActivateDeactivate_RangeBroadcaster_Success) // check interface configuration cmd_if_conf = range_broadcaster_->command_interface_configuration(); ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + ASSERT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); state_if_conf = range_broadcaster_->state_interface_configuration(); ASSERT_THAT(state_if_conf.names, SizeIs(1lu)); // did not change + ASSERT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); } TEST_F(RangeSensorBroadcasterTest, Update_RangeBroadcaster_Success) diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 81075d1082..0217567a26 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -28,41 +28,43 @@ class SteeringControllersLibraryTest }; // checking if all interfaces, command, state and reference are exported as expected -TEST_F(SteeringControllersLibraryTest, check_exported_intefaces) +TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_RIGHT_WHEEL], + cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_LEFT_WHEEL], + 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_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_RIGHT_WHEEL], + state_if_conf.names[STATE_STEER_RIGHT_WHEEL], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_LEFT_WHEEL], + 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 itfsTIME auto reference_interfaces = controller_->export_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 c72c61257a..8a0d21cc6c 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -73,7 +73,7 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; class TestableSteeringControllersLibrary : public steering_controllers_library::SteeringControllersLibrary { - FRIEND_TEST(SteeringControllersLibraryTest, check_exported_intefaces); + FRIEND_TEST(SteeringControllersLibraryTest, check_exported_interfaces); FRIEND_TEST(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout); public: diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 82ba924305..c555de53de 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -45,46 +45,47 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } -TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - // check ref itfsTIME - auto reference_interfaces = controller_->export_reference_interfaces(); - ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + // check ref itfs + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.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]); + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 422f399ad4..c2528e1fc9 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -61,7 +61,7 @@ class TestableTricycleSteeringController : public tricycle_steering_controller::TricycleSteeringController { FRIEND_TEST(TricycleSteeringControllerTest, all_parameters_set_configure_success); - FRIEND_TEST(TricycleSteeringControllerTest, check_exported_intefaces); + FRIEND_TEST(TricycleSteeringControllerTest, check_exported_interfaces); FRIEND_TEST(TricycleSteeringControllerTest, activate_success); FRIEND_TEST(TricycleSteeringControllerTest, update_success); FRIEND_TEST(TricycleSteeringControllerTest, deactivate_success); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index dd72332875..6f2913aeb8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -47,46 +47,48 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } -TEST_F(TricycleSteeringControllerTest, check_exported_intefaces) +TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { SetUpController(); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto command_intefaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + auto cmd_if_conf = controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_RIGHT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_TRACTION_LEFT_WHEEL], + cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - command_intefaces.names[CMD_STEER_WHEEL], + cmd_if_conf.names[CMD_STEER_WHEEL], preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - auto state_intefaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + auto state_if_conf = controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_RIGHT_WHEEL], + state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_TRACTION_LEFT_WHEEL], + state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - state_intefaces.names[STATE_STEER_AXIS], + state_if_conf.names[STATE_STEER_AXIS], controller_->front_wheels_state_names_[0] + "/" + 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()); + auto ref_if_conf = controller_->export_reference_interfaces(); + ASSERT_EQ(ref_if_conf.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]); + EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); } } From 864ed63b6be3c9d7d301017b66c82e5604e63814 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 1 Feb 2024 17:28:53 +0000 Subject: [PATCH 2/3] Delete pid_controller/test/test_pid_controller_preceding.cpp --- .../test/test_pid_controller_preceding.cpp | 104 ------------------ 1 file changed, 104 deletions(-) delete mode 100644 pid_controller/test/test_pid_controller_preceding.cpp diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp deleted file mode 100644 index 3e17e69286..0000000000 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ /dev/null @@ -1,104 +0,0 @@ -// 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. -// -// Authors: Daniel Azanov, Dr. Denis -// - -#include "test_pid_controller.hpp" - -#include -#include -#include -#include -#include - -using pid_controller::feedforward_mode_type; - -class PidControllerTest : public PidControllerFixture -{ -}; - -TEST_F(PidControllerTest, all_parameters_set_configure_success) -{ - SetUpController(); - - ASSERT_TRUE(controller_->params_.dof_names.empty()); - ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); - ASSERT_TRUE(controller_->params_.command_interface.empty()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); - ASSERT_THAT( - controller_->params_.reference_and_state_dof_names, - testing::ElementsAreArray(reference_and_state_dof_names_)); - ASSERT_THAT( - controller_->reference_and_state_dof_names_, - testing::ElementsAreArray(reference_and_state_dof_names_)); - ASSERT_EQ(controller_->params_.command_interface, command_interface_); -} - -TEST_F(PidControllerTest, 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(), dof_command_values_.size()); - for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) - { - EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); - } - 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(), dof_state_values_.size()); - size_t si_index = 0; - for (const auto & interface : state_interfaces_) - { - for (const auto & dof_name : reference_and_state_dof_names_) - { - EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); - ++si_index; - } - } - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - - // check ref itfs - auto ref_if_conf = controller_->export_reference_interfaces(); - ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); - size_t ri_index = 0; - for (const auto & interface : state_interfaces_) - { - for (const auto & dof_name : reference_and_state_dof_names_) - { - const std::string ref_itf_name = - std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); - ++ri_index; - } - } -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} From 0558e985121885dad716bdcbb876554f46f1b215 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Thu, 1 Feb 2024 17:29:06 +0000 Subject: [PATCH 3/3] Delete pid_controller/test/test_pid_controller.cpp --- pid_controller/test/test_pid_controller.cpp | 532 -------------------- 1 file changed, 532 deletions(-) delete mode 100644 pid_controller/test/test_pid_controller.cpp diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp deleted file mode 100644 index a44347f5f1..0000000000 --- a/pid_controller/test/test_pid_controller.cpp +++ /dev/null @@ -1,532 +0,0 @@ -// 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. -// -// Authors: Daniel Azanov, Dr. Denis -// - -#include "test_pid_controller.hpp" - -#include -#include -#include -#include -#include - -using pid_controller::feedforward_mode_type; - -class PidControllerTest : public PidControllerFixture -{ -}; - -TEST_F(PidControllerTest, all_parameters_set_configure_success) -{ - SetUpController(); - - ASSERT_TRUE(controller_->params_.dof_names.empty()); - ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); - ASSERT_TRUE(controller_->params_.command_interface.empty()); - ASSERT_TRUE(controller_->params_.reference_and_state_interfaces.empty()); - ASSERT_FALSE(controller_->params_.use_external_measured_states); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_THAT(controller_->params_.dof_names, testing::ElementsAreArray(dof_names_)); - ASSERT_TRUE(controller_->params_.reference_and_state_dof_names.empty()); - ASSERT_THAT(controller_->reference_and_state_dof_names_, testing::ElementsAreArray(dof_names_)); - for (const auto & dof_name : dof_names_) - { - ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].p, 1.0); - ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i, 2.0); - ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].d, 10.0); - ASSERT_FALSE(controller_->params_.gains.dof_names_map[dof_name].antiwindup); - ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_max, 5.0); - ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].i_clamp_min, -5.0); - ASSERT_EQ(controller_->params_.gains.dof_names_map[dof_name].feedforward_gain, 0.0); - } - ASSERT_EQ(controller_->params_.command_interface, command_interface_); - EXPECT_THAT( - controller_->params_.reference_and_state_interfaces, - testing::ElementsAreArray(state_interfaces_)); - ASSERT_FALSE(controller_->params_.use_external_measured_states); -} - -TEST_F(PidControllerTest, 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(), dof_command_values_.size()); - for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) - { - EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); - } - 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(), dof_state_values_.size()); - size_t si_index = 0; - for (const auto & interface : state_interfaces_) - { - for (const auto & dof_name : dof_names_) - { - EXPECT_EQ(state_if_conf.names[si_index], dof_name + "/" + interface); - ++si_index; - } - } - EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); - - // check ref itfs - auto ref_if_conf = controller_->export_reference_interfaces(); - ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); - size_t ri_index = 0; - for (const auto & interface : state_interfaces_) - { - for (const auto & dof_name : dof_names_) - { - const std::string ref_itf_name = - std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); - ++ri_index; - } - } -} - -TEST_F(PidControllerTest, activate_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - // check that the message is reset - auto msg = controller_->input_ref_.readFromNonRT(); - EXPECT_EQ((*msg)->values.size(), dof_names_.size()); - for (const auto & cmd : (*msg)->values) - { - EXPECT_TRUE(std::isnan(cmd)); - } - EXPECT_EQ((*msg)->values_dot.size(), dof_names_.size()); - for (const auto & cmd : (*msg)->values_dot) - { - EXPECT_TRUE(std::isnan(cmd)); - } - - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } -} - -TEST_F(PidControllerTest, update_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(PidControllerTest, deactivate_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); -} - -TEST_F(PidControllerTest, reactivate_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); - ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); -} - -TEST_F(PidControllerTest, test_feedforward_mode_service) -{ - SetUpController(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - // initially set to OFF - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - // should stay false - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - - // set to true - ASSERT_NO_THROW(call_service(true, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - - // set back to false - ASSERT_NO_THROW(call_service(false, executor)); - ASSERT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); -} - -TEST_F(PidControllerTest, test_update_logic_feedforward_off) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_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()); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } - - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); - - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); - } - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - EXPECT_EQ( - controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - } -} - -TEST_F(PidControllerTest, test_update_logic_feedforward_on) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_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()); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - for (const auto & interface : controller_->reference_interfaces_) - { - EXPECT_TRUE(std::isnan(interface)); - } - - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); - - controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); - } - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - EXPECT_EQ( - controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - } -} - -TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - controller_->set_chained_mode(true); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(controller_->is_in_chained_mode()); - - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); - - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); - } - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ASSERT_TRUE(controller_->is_in_chained_mode()); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - EXPECT_EQ( - controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - } -} - -TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - controller_->set_chained_mode(true); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(controller_->is_in_chained_mode()); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - - std::shared_ptr msg = std::make_shared(); - msg->dof_names = dof_names_; - msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - msg->values[i] = dof_command_values_[i]; - } - msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); - controller_->input_ref_.writeFromNonRT(msg); - - controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); - } - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ASSERT_TRUE(controller_->is_in_chained_mode()); - EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - EXPECT_EQ( - controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) - { - EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - } -} - -/** - * @brief check default calculation with angle_wraparound turned off - */ -TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - controller_->set_chained_mode(true); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(controller_->is_in_chained_mode()); - - // write reference interface so that the values are would be wrapped - - // run update - - // check the result of the commands - the values are not wrapped -} - -/** - * @brief check default calculation with angle_wraparound turned off - */ -TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on) -{ - SetUpController(); - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(controller_->get_node()->get_node_base_interface()); - executor.add_node(service_caller_node_->get_node_base_interface()); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - controller_->set_chained_mode(true); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(controller_->is_in_chained_mode()); - - // write reference interface so that the values are would be wrapped - - // run update - - // check the result of the commands - the values are wrapped -} - -TEST_F(PidControllerTest, subscribe_and_get_messages_success) -{ - SetUpController(); - - ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ControllerStateMsg msg; - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); - for (size_t i = 0; i < dof_names_.size(); ++i) - { - ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); - EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); - ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); - } -} - -TEST_F(PidControllerTest, receive_message_and_publish_updated_status) -{ - 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); - ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ControllerStateMsg msg; - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); - for (size_t i = 0; i < dof_names_.size(); ++i) - { - ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); - EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); - ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); - } - - for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) - { - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); - } - - publish_commands(); - ASSERT_TRUE(controller_->wait_for_commands(executor)); - - for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) - { - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); - } - - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) - { - ASSERT_EQ(controller_->reference_interfaces_[i], 0.45); - } - - subscribe_and_get_messages(msg); - - ASSERT_EQ(msg.dof_states.size(), dof_names_.size()); - for (size_t i = 0; i < dof_names_.size(); ++i) - { - ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); - ASSERT_EQ(msg.dof_states[i].reference, 0.45); - ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); - } -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -}