From c7c2f52c8d595c003e07504b666c57b4a2d62dde Mon Sep 17 00:00:00 2001 From: Manuel M Date: Thu, 29 Aug 2024 15:10:42 +0200 Subject: [PATCH] continue adapting controllers --- .../test_ackermann_steering_controller.cpp | 70 ++-- .../test_ackermann_steering_controller.hpp | 66 +-- ...kermann_steering_controller_preceeding.cpp | 6 +- .../admittance_controller.hpp | 5 +- .../test/test_bicycle_steering_controller.cpp | 45 +- .../test/test_bicycle_steering_controller.hpp | 35 +- ...bicycle_steering_controller_preceeding.cpp | 6 +- .../src/joint_trajectory_controller.cpp | 39 +- .../test/test_trajectory_actions.cpp | 18 +- .../test/test_trajectory_controller.cpp | 389 ++++++++++-------- .../test/test_trajectory_controller_utils.hpp | 135 +++--- .../steering_controllers_library.hpp | 2 +- .../test_tricycle_steering_controller.cpp | 58 +-- .../test_tricycle_steering_controller.hpp | 49 ++- ...ricycle_steering_controller_preceeding.cpp | 6 +- 15 files changed, 508 insertions(+), 421 deletions(-) diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index c04e87be95..8d8080ee66 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -51,7 +51,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto cmd_if_conf = controller_->command_interface_configuration(); - ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_size_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); @@ -67,7 +67,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); - ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_size_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); @@ -83,7 +83,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); + auto reference_interfaces = controller_->export_reference_interface_descriptions(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { @@ -140,9 +140,9 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success) 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); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -173,23 +173,25 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic) // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); - for (const auto & interface : controller_->reference_interfaces_) + EXPECT_EQ(controller_->ordered_reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(std::isnan(interface->get_value())); } } @@ -204,8 +206,8 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); - controller_->reference_interfaces_[0] = 0.1; - controller_->reference_interfaces_[1] = 0.2; + controller_->ordered_reference_interfaces_[0]->set_value(0.1); + controller_->ordered_reference_interfaces_[1]->set_value(0.2); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -213,23 +215,25 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained) // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, - COMMON_THRESHOLD); + controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), + 1.4179821977774734, COMMON_THRESHOLD); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); - for (const auto & interface : controller_->reference_interfaces_) + EXPECT_EQ(controller_->ordered_reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(std::isnan(interface->get_value())); } } @@ -263,16 +267,16 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 59637a072f..bba090c186 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -80,7 +80,7 @@ class TestableAckermannSteeringController controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override { - auto ref_itfs = on_export_reference_interfaces(); + auto ref_itfs = export_reference_interface_descriptions(); return ackermann_steering_controller::AckermannSteeringController::on_activate(previous_state); } @@ -147,51 +147,55 @@ class AckermannSteeringControllerFixture : public ::testing::Test } std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, - &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[0], + hardware_interface::InterfaceInfo(traction_interface_name_, "double", "1.1")))); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[1], steering_interface_name_, - &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[1], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "3.3")))); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, - &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + front_wheels_names_[0], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "2.2")))); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[1], steering_interface_name_, - &joint_command_values_[CMD_STEER_LEFT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + front_wheels_names_[1], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "4.4")))); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; - state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[0], + hardware_interface::InterfaceInfo(traction_interface_name_, "double", "0.5")))); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[1], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[1], + hardware_interface::InterfaceInfo(traction_interface_name_, "double", "0.5")))); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, - &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + front_wheels_names_[0], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "0.0")))); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[1], steering_interface_name_, - &joint_state_values_[STATE_STEER_LEFT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + front_wheels_names_[1], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "0.0")))); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -293,8 +297,8 @@ class AckermannSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; - std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; + const size_t joint_command_values_size_ = 4; + const size_t joint_state_values_size_ = 4; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup 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 96dd20d80e..01a9f1f94d 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -53,7 +53,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto cmd_if_conf = controller_->command_interface_configuration(); - ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_size_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); @@ -69,7 +69,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); - ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_size_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); @@ -85,7 +85,7 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); + auto reference_interfaces = controller_->export_reference_interface_descriptions(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9be6c3298c..a4f33bacd2 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -87,7 +87,10 @@ class AdmittanceController : public controller_interface::ChainableControllerInt const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::vector on_export_reference_interfaces() override; + std::vector export_state_interface_descriptions() + override; + std::vector export_reference_interface_descriptions() + override; controller_interface::return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) override; diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 573992b24e..eb76b3c438 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -49,7 +49,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto cmd_if_conf = controller_->command_interface_configuration(); - ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_size_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( @@ -57,7 +57,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); - ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_size_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); @@ -67,7 +67,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); + auto reference_interfaces = controller_->export_reference_interface_descriptions(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { @@ -124,9 +124,9 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success) 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); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -156,16 +156,19 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); - for (const auto & interface : controller_->reference_interfaces_) + EXPECT_EQ(controller_->ordered_reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(std::isnan(interface->get_value())); } } @@ -180,24 +183,27 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); - controller_->reference_interfaces_[0] = 0.1; - controller_->reference_interfaces_[1] = 0.2; + controller_->ordered_reference_interfaces_[0]->set_value(0.1); + controller_->ordered_reference_interfaces_[1]->set_value(0.2); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); - for (const auto & interface : controller_->reference_interfaces_) + EXPECT_EQ(controller_->ordered_reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(std::isnan(interface->get_value())); } } @@ -243,9 +249,10 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, + COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 65f1691a3b..605ff90a9d 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -78,7 +78,7 @@ class TestableBicycleSteeringController controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override { - auto ref_itfs = on_export_reference_interfaces(); + auto ref_itfs = export_reference_interface_descriptions(); return bicycle_steering_controller::BicycleSteeringController::on_activate(previous_state); } @@ -145,27 +145,31 @@ class BicycleSteeringControllerFixture : public ::testing::Test } std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[0], + hardware_interface::InterfaceInfo(traction_interface_name_, "double", "1.1")))); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + front_wheels_names_[0], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "2.2")))); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; - state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[0], + hardware_interface::InterfaceInfo(traction_interface_name_, "double", "3.3")))); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + front_wheels_names_[0], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "0.5")))); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -260,8 +264,9 @@ class BicycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {3.3, 0.5}; - std::array joint_command_values_ = {1.1, 2.2}; + const size_t joint_command_values_size_ = 2; + const size_t joint_state_values_size_ = 2; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; 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 0bc03f4886..c8e712ad08 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -51,7 +51,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto cmd_if_conf = controller_->command_interface_configuration(); - ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_size_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); @@ -61,7 +61,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); - ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_size_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); @@ -71,7 +71,7 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto reference_interfaces = controller_->export_reference_interfaces(); + auto reference_interfaces = controller_->export_reference_interface_descriptions(); ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 68653b72aa..70c3344bcd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -410,7 +410,10 @@ controller_interface::return_type JointTrajectoryController::update( void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) + [&]( + std::vector & trajectory_point_interface, + const std::vector> & + joint_interface) { for (size_t index = 0; index < dof_; ++index) { @@ -449,19 +452,26 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto bool has_values = true; auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) + [&]( + std::vector & trajectory_point_interface, + const std::vector> & + joint_interface) { for (size_t index = 0; index < dof_; ++index) { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } }; - auto interface_has_values = [](const auto & joint_interface) + auto interface_has_values = + [](const std::vector> & + joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), [](const auto & interface) - { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + joint_interface.begin(), joint_interface.end(), + []( + const std::reference_wrapper & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; // Assign values from the command interfaces as state. Therefore needs check for both. @@ -519,19 +529,26 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( bool has_values = true; auto assign_point_from_interface = - [&](std::vector & trajectory_point_interface, const auto & joint_interface) + [&]( + std::vector & trajectory_point_interface, + const std::vector> & + joint_interface) { for (size_t index = 0; index < dof_; ++index) { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } }; - auto interface_has_values = [](const auto & joint_interface) + auto interface_has_values = + [](const std::vector> & + joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), [](const auto & interface) - { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); + joint_interface.begin(), joint_interface.end(), + []( + const std::reference_wrapper & interface) + { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; // Assign values from the command interfaces as command. diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 164f7d0e11..61b1894992 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -59,13 +59,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest } void SetUpExecutor( - const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0) + const std::vector & parameters = {}, double kp = 0.0, double ff = 1.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController( - executor_, parameters, separate_cmd_and_state_values, kp, ff); + SetUpAndActivateTrajectoryController(executor_, parameters, kp, ff); SetUpActionClient(); @@ -221,7 +219,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa // deactivate velocity tolerance std::vector params = { rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; - SetUpExecutor(params, false, 1.0, 0.0); + SetUpExecutor(params, 1.0, 0.0); SetUpControllerHardware(); std::shared_future gh_future; @@ -257,7 +255,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_ve std::vector params = { rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpExecutor(params, false, 1.0, 0.0); + SetUpExecutor(params, 1.0, 0.0); SetUpControllerHardware(); std::shared_future gh_future; @@ -294,7 +292,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal // deactivate velocity tolerance std::vector params = { rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; - SetUpExecutor({params}, false, 1.0, 0.0); + SetUpExecutor({params}, 1.0, 0.0); SetUpControllerHardware(); // add feedback @@ -341,7 +339,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_vel std::vector params = { rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; - SetUpExecutor(params, false, 1.0, 0.0); + SetUpExecutor(params, 1.0, 0.0); SetUpControllerHardware(); // add feedback @@ -826,7 +824,9 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - std::vector cancelled_position{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; + std::vector cancelled_position{ + pos_state_interfaces_[0].get_value(), pos_state_interfaces_[1].get_value(), + pos_state_interfaces_[2].get_value()}; // run an update updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f188c0f04b..587c7b2070 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -72,9 +72,12 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) rclcpp::Time(static_cast(0.5 * 1e9)), rclcpp::Duration::from_seconds(0.5)); // hw position == 0 because controller is not activated - EXPECT_EQ(0.0, joint_pos_[0]); - EXPECT_EQ(0.0, joint_pos_[1]); - EXPECT_EQ(0.0, joint_pos_[2]); + EXPECT_EQ(INITIAL_POS_JOINT1, pos_state_interfaces_[0].get_value()); + EXPECT_EQ(INITIAL_POS_JOINT2, pos_state_interfaces_[1].get_value()); + EXPECT_EQ(INITIAL_POS_JOINT3, pos_state_interfaces_[2].get_value()); + EXPECT_EQ(0.0, pos_cmd_interfaces_[0].get_value()); + EXPECT_EQ(0.0, pos_cmd_interfaces_[1].get_value()); + EXPECT_EQ(0.0, pos_cmd_interfaces_[2].get_value()); executor.cancel(); } @@ -185,9 +188,12 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param state = ActivateTrajectoryController(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); - EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]); - EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]); - EXPECT_EQ(INITIAL_POS_JOINT3, joint_pos_[2]); + EXPECT_EQ(INITIAL_POS_JOINT1, pos_state_interfaces_[0].get_value()); + EXPECT_EQ(INITIAL_POS_JOINT2, pos_state_interfaces_[1].get_value()); + EXPECT_EQ(INITIAL_POS_JOINT3, pos_state_interfaces_[2].get_value()); + EXPECT_EQ(0.0, pos_cmd_interfaces_[0].get_value()); + EXPECT_EQ(0.0, pos_cmd_interfaces_[1].get_value()); + EXPECT_EQ(0.0, pos_cmd_interfaces_[2].get_value()); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); @@ -211,13 +217,15 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param EXPECT_TRUE(traj_controller_->has_active_traj()); if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(points.at(0).at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points.at(0).at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points.at(0).at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(0), pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(1), pos_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(points.at(0).at(2), pos_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD); } // deactivate - std::vector deactivated_positions{joint_pos_[0], joint_pos_[1], joint_pos_[2]}; + std::vector deactivated_positions{ + pos_cmd_interfaces_[0].get_value(), pos_cmd_interfaces_[1].get_value(), + pos_cmd_interfaces_[2].get_value()}; state = traj_controller_->get_node()->deactivate(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); // it should be holding the current point @@ -228,7 +236,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // wait so controller would have processed the third point when reactivated -> but it shouldn't std::this_thread::sleep_for(std::chrono::milliseconds(3000)); - state = ActivateTrajectoryController(false, deactivated_positions); + state = ActivateTrajectoryController(deactivated_positions); ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); // it should still be holding the position at time of deactivation @@ -461,7 +469,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_tru rclcpp::executors::MultiThreadedExecutor executor; std::vector params = {}; SetUpAndActivateTrajectoryController( - executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + executor, params, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous); size_t n_joints = joint_names_.size(); @@ -553,7 +561,7 @@ TEST_P(TrajectoryControllerTestParameterized, compute_error_angle_wraparound_fal rclcpp::executors::MultiThreadedExecutor executor; std::vector params = {}; SetUpAndActivateTrajectoryController( - executor, params, true, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + executor, params, 0.0, 1.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); size_t n_joints = joint_names_.size(); @@ -637,7 +645,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun constexpr double k_p = 10.0; std::vector params = {}; SetUpAndActivateTrajectoryController( - executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + executor, params, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); subscribeToState(executor); @@ -681,9 +689,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], pos_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], pos_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) @@ -693,22 +701,22 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun { // we expect u = k_p * (s_d-s) for positions EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), + vel_cmd_interfaces_[0].get_value(), k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), + vel_cmd_interfaces_[1].get_value(), k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), + vel_cmd_interfaces_[2].get_value(), k_p * COMMON_THRESHOLD); } else { // interpolated points_velocities only // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); + EXPECT_LT(0.0, vel_cmd_interfaces_[0].get_value()); + EXPECT_LT(0.0, vel_cmd_interfaces_[1].get_value()); + EXPECT_LT(0.0, vel_cmd_interfaces_[2].get_value()); } } @@ -717,14 +725,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun // with effort command interface, use_closed_loop_pid_adapter is always true // we expect u = k_p * (s_d-s) for positions EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), + eff_cmd_interfaces_[0].get_value(), k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), + eff_cmd_interfaces_[1].get_value(), k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), + eff_cmd_interfaces_[2].get_value(), k_p * COMMON_THRESHOLD); } executor.cancel(); @@ -739,7 +747,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) constexpr double k_p = 10.0; std::vector params = {}; SetUpAndActivateTrajectoryController( - executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, + executor, params, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_continuous); size_t n_joints = joint_names_.size(); @@ -785,9 +793,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) if (traj_controller_->has_position_command_interface()) { // check command interface - EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], pos_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], pos_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) @@ -797,24 +805,25 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) { // we expect u = k_p * (s_d-s) for joint0 and joint1 EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), + vel_cmd_interfaces_[0].get_value(), k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), + vel_cmd_interfaces_[1].get_value(), k_p * COMMON_THRESHOLD); // is error of positions[2] wrapped around? - EXPECT_GT(0.0, joint_vel_[2]); // direction change because of angle wrap + EXPECT_GT( + 0.0, vel_cmd_interfaces_[2].get_value()); // direction change because of angle wrap EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), + vel_cmd_interfaces_[2].get_value(), k_p * COMMON_THRESHOLD); } else { // interpolated points_velocities only // check command interface - EXPECT_LT(0.0, joint_vel_[0]); - EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); + EXPECT_LT(0.0, vel_cmd_interfaces_[0].get_value()); + EXPECT_LT(0.0, vel_cmd_interfaces_[1].get_value()); + EXPECT_LT(0.0, vel_cmd_interfaces_[2].get_value()); } } @@ -823,16 +832,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) // with effort command interface, use_closed_loop_pid_adapter is always true // we expect u = k_p * (s_d-s) for joint0 and joint1 EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), + eff_cmd_interfaces_[0].get_value(), k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), + eff_cmd_interfaces_[1].get_value(), k_p * COMMON_THRESHOLD); // is error of positions[2] wrapped around? - EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_GT(0.0, eff_cmd_interfaces_[2].get_value()); EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * COMMON_THRESHOLD); + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), + eff_cmd_interfaces_[2].get_value(), k_p * COMMON_THRESHOLD); } executor.cancel(); @@ -1092,9 +1101,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) for (size_t dof = 0; dof < 3; dof++) { traj_msg.points[0].velocities[dof] = - (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + (traj_msg.points[0].positions[dof] - + pos_cmd_interfaces_[jumble_map[dof]].get_value()) / + dt; traj_msg.points[0].accelerations[dof] = - (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + (traj_msg.points[0].velocities[dof] - + vel_cmd_interfaces_[jumble_map[dof]].get_value()) / + dt; } trajectory_publisher_->publish(traj_msg); @@ -1105,9 +1118,12 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(points_positions.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(points_positions.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(points_positions.at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR( + points_positions.at(0), pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR( + points_positions.at(1), pos_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR( + points_positions.at(2), pos_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) @@ -1115,24 +1131,24 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with // feedforward term only - EXPECT_GT(0.0, joint_vel_[0]); - EXPECT_GT(0.0, joint_vel_[1]); - EXPECT_GT(0.0, joint_vel_[2]); + EXPECT_GT(0.0, vel_cmd_interfaces_[0].get_value()); + EXPECT_GT(0.0, vel_cmd_interfaces_[1].get_value()); + EXPECT_GT(0.0, vel_cmd_interfaces_[2].get_value()); } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_GT(0.0, joint_acc_[0]); - EXPECT_GT(0.0, joint_acc_[1]); - EXPECT_GT(0.0, joint_acc_[2]); + EXPECT_GT(0.0, acc_cmd_interfaces_[0].get_value()); + EXPECT_GT(0.0, acc_cmd_interfaces_[1].get_value()); + EXPECT_GT(0.0, acc_cmd_interfaces_[2].get_value()); } if (traj_controller_->has_effort_command_interface()) { // effort should be nonzero, because we use PID with feedforward term - EXPECT_GT(0.0, joint_eff_[0]); - EXPECT_GT(0.0, joint_eff_[1]); - EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_GT(0.0, eff_cmd_interfaces_[0].get_value()); + EXPECT_GT(0.0, eff_cmd_interfaces_[1].get_value()); + EXPECT_GT(0.0, eff_cmd_interfaces_[2].get_value()); } } @@ -1147,9 +1163,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - const double initial_joint1_cmd = joint_pos_[0]; - const double initial_joint2_cmd = joint_pos_[1]; - const double initial_joint3_cmd = joint_pos_[2]; + const double initial_joint1_cmd = pos_cmd_interfaces_[0].get_value(); + const double initial_joint2_cmd = pos_cmd_interfaces_[0].get_value(); + const double initial_joint3_cmd = pos_cmd_interfaces_[0].get_value(); const double dt = 0.25; trajectory_msgs::msg::JointTrajectory traj_msg; @@ -1170,9 +1186,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) for (size_t dof = 0; dof < 2; dof++) { traj_msg.points[0].velocities[dof] = - (traj_msg.points[0].positions[dof] - joint_pos_[jumble_map[dof]]) / dt; + (traj_msg.points[0].positions[dof] - + pos_cmd_interfaces_[jumble_map[dof]].get_value()) / + dt; traj_msg.points[0].accelerations[dof] = - (traj_msg.points[0].velocities[dof] - joint_vel_[jumble_map[dof]]) / dt; + (traj_msg.points[0].velocities[dof] - + vel_cmd_interfaces_[jumble_map[dof]].get_value()) / + dt; } trajectory_publisher_->publish(traj_msg); @@ -1183,9 +1203,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) + EXPECT_NEAR( + traj_msg.points[0].positions[1], pos_cmd_interfaces_[0].get_value(), + COMMON_THRESHOLD); + EXPECT_NEAR( + traj_msg.points[0].positions[0], pos_cmd_interfaces_[1].get_value(), + COMMON_THRESHOLD); + EXPECT_NEAR(initial_joint3_cmd, pos_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD) << "Joint 3 command should be current position"; } @@ -1193,11 +1217,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the velocity // joint rotates forward - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); - EXPECT_NEAR(0.0, joint_vel_[2], COMMON_THRESHOLD) + EXPECT_TRUE(is_same_sign_or_zero( + traj_msg.points[0].positions[0] - initial_joint2_cmd, + vel_cmd_interfaces_[0].get_value())); + EXPECT_TRUE(is_same_sign_or_zero( + traj_msg.points[0].positions[1] - initial_joint1_cmd, + vel_cmd_interfaces_[1].get_value())); + EXPECT_NEAR(0.0, vel_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } @@ -1205,15 +1231,17 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the acceleration // joint rotates forward - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_acc_[0])) + EXPECT_TRUE(is_same_sign_or_zero( + traj_msg.points[0].positions[0] - initial_joint2_cmd, + acc_cmd_interfaces_[0].get_value())) << "Joint1: " << traj_msg.points[0].positions[0] - initial_joint2_cmd << " vs. " - << joint_acc_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_acc_[1])) + << acc_cmd_interfaces_[0].get_value(); + EXPECT_TRUE(is_same_sign_or_zero( + traj_msg.points[0].positions[1] - initial_joint1_cmd, + acc_cmd_interfaces_[1].get_value())) << "Joint2: " << traj_msg.points[0].positions[1] - initial_joint1_cmd << " vs. " - << joint_acc_[1]; - EXPECT_NEAR(0.0, joint_acc_[2], COMMON_THRESHOLD) + << acc_cmd_interfaces_[1].get_value(); + EXPECT_NEAR(0.0, acc_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD) << "Joint 3 acc should be 0.0 since it's not in the goal"; } @@ -1221,11 +1249,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) { // estimate the sign of the effort // joint rotates forward - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) + EXPECT_TRUE(is_same_sign_or_zero( + traj_msg.points[0].positions[0] - initial_joint2_cmd, + eff_cmd_interfaces_[0].get_value())); + EXPECT_TRUE(is_same_sign_or_zero( + traj_msg.points[0].positions[1] - initial_joint1_cmd, + eff_cmd_interfaces_[1].get_value())); + EXPECT_NEAR(0.0, eff_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD) << "Joint 3 effort should be 0.0 since it's not in the goal"; } @@ -1243,9 +1273,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters}); - const double initial_joint1_cmd = joint_pos_[0]; - const double initial_joint2_cmd = joint_pos_[1]; - const double initial_joint3_cmd = joint_pos_[2]; + const double initial_joint1_cmd = pos_cmd_interfaces_[0].get_value(); + const double initial_joint2_cmd = pos_cmd_interfaces_[1].get_value(); + const double initial_joint3_cmd = pos_cmd_interfaces_[2].get_value(); trajectory_msgs::msg::JointTrajectory traj_msg; { @@ -1271,41 +1301,41 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(initial_joint1_cmd, joint_pos_[0], COMMON_THRESHOLD) + EXPECT_NEAR(initial_joint1_cmd, pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint2_cmd, joint_pos_[1], COMMON_THRESHOLD) + EXPECT_NEAR(initial_joint2_cmd, pos_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], COMMON_THRESHOLD) + EXPECT_NEAR(initial_joint3_cmd, pos_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD) << "All joints command should be current position because goal was rejected"; } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_NEAR(INITIAL_VEL_JOINTS[0], joint_vel_[0], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_VEL_JOINTS[0], vel_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[1], joint_vel_[1], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_VEL_JOINTS[1], vel_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_VEL_JOINTS[2], joint_vel_[2], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_VEL_JOINTS[2], vel_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD) << "All joints velocities should be 0.0 because goal was rejected"; } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_NEAR(INITIAL_ACC_JOINTS[0], joint_acc_[0], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_ACC_JOINTS[0], acc_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[1], joint_acc_[1], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_ACC_JOINTS[1], acc_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_ACC_JOINTS[2], joint_acc_[2], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_ACC_JOINTS[2], acc_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD) << "All joints accelerations should be 0.0 because goal was rejected"; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_NEAR(INITIAL_EFF_JOINTS[0], joint_eff_[0], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_EFF_JOINTS[0], eff_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[1], joint_eff_[1], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_EFF_JOINTS[1], eff_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; - EXPECT_NEAR(INITIAL_EFF_JOINTS[2], joint_eff_[2], COMMON_THRESHOLD) + EXPECT_NEAR(INITIAL_EFF_JOINTS[2], eff_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD) << "All joints efforts should be 0.0 because goal was rejected"; } @@ -1514,7 +1544,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) RCLCPP_INFO(traj_controller_->get_node()->get_logger(), "Sending new trajectory"); points_partial_new_velocities[0][0] = - std::copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + std::copysign(0.15, points_partial_new[0][0] - pos_state_interfaces_[0].get_value()); publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); // Replaced trajectory is a mix of previous and current goal @@ -1676,11 +1706,11 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // JTC is executing trajectory in open-loop therefore: // - internal state does not have to be updated (in this test-case it shouldn't) // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT1, pos_state_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + pos_state_interfaces_[0].set_value(first_goal[0] - state_from_command_offset); // Move joint further in the same direction as before (to the second goal) points = {{second_goal}}; @@ -1689,29 +1719,29 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in opposite direction from command // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], pos_state_interfaces_[0].get_value(), COMMON_THRESHOLD); auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // Expect backward commands at first, consider advancement of the trajectory // exact value is not directly predictable, because of the spline interpolation -> increase // tolerance EXPECT_NEAR( - joint_state_pos_[0] + (second_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], - 0.1); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); + pos_state_interfaces_[0] + (second_goal[0] - pos_state_interfaces_[0]) * trajectory_frac, + pos_cmd_interfaces_[0].get_value(), 0.1); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), pos_state_interfaces_[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), pos_state_interfaces_[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), pos_state_interfaces_[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); // Finally the second goal will be commanded/reached updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(second_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + pos_state_interfaces_[0].set_value(second_goal[0] - state_from_command_offset); // Move joint back to the first goal points = {{first_goal}}; @@ -1720,24 +1750,24 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // One the first update(s) there should be a "jump" in the goal direction from command // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(second_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // Expect backward commands at first, consider advancement of the trajectory EXPECT_NEAR( - joint_state_pos_[0] + (first_goal[0] - joint_state_pos_[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); + pos_state_interfaces_[0] + (first_goal[0] - pos_state_interfaces_[0]) * trajectory_frac, + pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), pos_state_interfaces_[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), pos_state_interfaces_[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), pos_state_interfaces_[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); // Finally the first goal will be commanded/reached updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); executor.cancel(); } @@ -1774,11 +1804,11 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // JTC is executing trajectory in open-loop therefore: // - internal state does not have to be updated (in this test-case it shouldn't) // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(INITIAL_POS_JOINT1, pos_state_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + pos_state_interfaces_[0].set_value(first_goal[0] - state_from_command_offset); // Move joint further in the same direction as before (to the second goal) points = {{second_goal}}; @@ -1787,27 +1817,27 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in opposite direction from // command (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); // There should not be backward commands EXPECT_NEAR( - first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + first_goal[0] + (second_goal[0] - first_goal[0]) * trajectory_frac, + pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), second_goal[0]); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), second_goal[0]); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), second_goal[0]); // Finally the second goal will be commanded/reached updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(second_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + pos_state_interfaces_[0].set_value(second_goal[0] - state_from_command_offset); // Move joint back to the first goal points = {{first_goal}}; @@ -1816,24 +1846,24 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e // One the first update(s) there **should not** be a "jump" in the goal direction from // command (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(second_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); // There should not be a jump toward commands EXPECT_NEAR( - second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, joint_pos_[0], - COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); + second_goal[0] + (first_goal[0] - second_goal[0]) * trajectory_frac, + pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), second_goal[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), second_goal[0]); end_time = updateControllerAsync(rclcpp::Duration::from_seconds(0.01), end_time); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(pos_cmd_interfaces_[0].get_value(), first_goal[0]); + EXPECT_LT(pos_cmd_interfaces_[0].get_value(), second_goal[0]); // Finally the first goal will be commanded/reached updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); executor.cancel(); } @@ -1851,8 +1881,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co std::vector initial_acc_cmd{3, std::numeric_limits::quiet_NaN()}; SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, - initial_acc_cmd); + executor, {is_open_loop_parameters}, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from state interfaces // (command interface are NaN) @@ -1861,18 +1890,20 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co for (size_t i = 0; i < 3; ++i) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.positions[i], pos_state_interfaces_[i].get_value()); // check velocity if (traj_controller_->has_velocity_state_interface()) { - EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + EXPECT_EQ( + current_state_when_offset.velocities[i], vel_state_interfaces_[i].get_value()); } // check acceleration if (traj_controller_->has_acceleration_state_interface()) { - EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); + EXPECT_EQ( + current_state_when_offset.accelerations[i], acc_state_interfaces_[i].get_value()); } } @@ -1895,8 +1926,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); } SetUpAndActivateTrajectoryController( - executor, {is_open_loop_parameters}, true, 0., 1., initial_pos_cmd, initial_vel_cmd, - initial_acc_cmd); + executor, {is_open_loop_parameters}, 0., 1., initial_pos_cmd, initial_vel_cmd, initial_acc_cmd); // no call of update method, so the values should be read from command interfaces @@ -1925,9 +1955,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co else { // should have set it to the state interface instead - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); - EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); + EXPECT_EQ( + current_state_when_offset.positions[i], + pos_state_interfaces_[i].get_value()); + EXPECT_EQ( + current_state_when_offset.velocities[i], + vel_state_interfaces_[i].get_value()); + EXPECT_EQ( + current_state_when_offset.accelerations[i], + acc_state_interfaces_[i].get_value()); } } else @@ -1940,8 +1976,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co else { // should have set it to the state interface instead - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); - EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + EXPECT_EQ(current_state_when_offset.positions[i], pos_state_interfaces_[i]); + EXPECT_EQ( + current_state_when_offset.velocities[i], vel_state_interfaces_[i].get_value()); } } else @@ -1953,7 +1990,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co else { // should have set it to the state interface instead - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.positions[i], pos_state_interfaces_[i]); } } @@ -1987,7 +2024,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now - expectCommandPoint(joint_state_pos_); + expectCommandPoint( + {pos_state_interfaces_[0].get_value(), pos_state_interfaces_[1].get_value(), + pos_state_interfaces_[2].get_value()}); } TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) @@ -2019,11 +2058,15 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now - expectCommandPoint(joint_state_pos_); + expectCommandPoint( + {pos_state_interfaces_[0].get_value(), pos_state_interfaces_[1].get_value(), + pos_state_interfaces_[2].get_value()}); // what happens if we wait longer but it harms the tolerance again? - auto hold_position = joint_state_pos_; - joint_state_pos_.at(0) = -3.3; + std::vector hold_position{ + pos_state_interfaces_[0].get_value(), pos_state_interfaces_[1].get_value(), + pos_state_interfaces_[2].get_value()}; + pos_state_interfaces_.at(0).set_value(-3.3); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point expectCommandPoint(hold_position); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 9534bb6c00..3400cae996 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -232,13 +232,6 @@ class TrajectoryControllerTest : public ::testing::Test joint_names_ = {"joint1", "joint2", "joint3"}; command_joint_names_ = { "following_controller/joint1", "following_controller/joint2", "following_controller/joint3"}; - joint_pos_.resize(joint_names_.size(), 0.0); - joint_state_pos_.resize(joint_names_.size(), 0.0); - joint_vel_.resize(joint_names_.size(), 0.0); - joint_state_vel_.resize(joint_names_.size(), 0.0); - joint_acc_.resize(joint_names_.size(), 0.0); - joint_state_acc_.resize(joint_names_.size(), 0.0); - joint_eff_.resize(joint_names_.size(), 0.0); // Default interface values - they will be overwritten by parameterized tests command_interface_types_ = {"position"}; state_interface_types_ = {"position", "velocity"}; @@ -299,7 +292,7 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, + double k_p = 0.0, double ff = 1.0, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, @@ -326,12 +319,10 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->get_node()->configure(); ActivateTrajectoryController( - separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, - initial_eff_joints); + initial_pos_joints, initial_vel_joints, initial_acc_joints, initial_eff_joints); } rclcpp_lifecycle::State ActivateTrajectoryController( - bool separate_cmd_and_state_values = false, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, @@ -348,24 +339,38 @@ class TrajectoryControllerTest : public ::testing::Test acc_state_interfaces_.reserve(joint_names_.size()); for (size_t i = 0; i < joint_names_.size(); ++i) { - pos_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i])); - vel_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); - acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); - eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); - - pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], hardware_interface::HW_IF_POSITION, - separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i])); - vel_state_interfaces_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], hardware_interface::HW_IF_VELOCITY, - separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i])); - acc_state_interfaces_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], hardware_interface::HW_IF_ACCELERATION, - separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i])); + pos_cmd_interfaces_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + joint_names_[i], + hardware_interface::InterfaceInfo(hardware_interface::HW_IF_POSITION, "double", "0.0")))); + vel_cmd_interfaces_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + joint_names_[i], + hardware_interface::InterfaceInfo(hardware_interface::HW_IF_VELOCITY, "double", "0.0")))); + acc_cmd_interfaces_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + joint_names_[i], hardware_interface::InterfaceInfo( + hardware_interface::HW_IF_ACCELERATION, "double", "0.0")))); + eff_cmd_interfaces_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + joint_names_[i], + hardware_interface::InterfaceInfo(hardware_interface::HW_IF_EFFORT, "double", "0.0")))); + + pos_state_interfaces_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + joint_names_[i], + hardware_interface::InterfaceInfo( + hardware_interface::HW_IF_POSITION, "double", std::to_string(INITIAL_POS_JOINT1))))); + vel_state_interfaces_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + joint_names_[i], + hardware_interface::InterfaceInfo( + hardware_interface::HW_IF_VELOCITY, "double", std::to_string(INITIAL_POS_JOINT2))))); + acc_state_interfaces_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + joint_names_[i], hardware_interface::InterfaceInfo( + hardware_interface::HW_IF_ACCELERATION, "double", + std::to_string(INITIAL_POS_JOINT3))))); // Add to export lists and set initial values cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); @@ -376,12 +381,7 @@ class TrajectoryControllerTest : public ::testing::Test cmd_interfaces.back().set_value(initial_acc_joints[i]); cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); cmd_interfaces.back().set_value(initial_eff_joints[i]); - if (separate_cmd_and_state_values) - { - joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; - joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; - joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; - } + state_interfaces.emplace_back(pos_state_interfaces_.back()); state_interfaces.emplace_back(vel_state_interfaces_.back()); state_interfaces.emplace_back(acc_state_interfaces_.back()); @@ -602,30 +602,30 @@ class TrajectoryControllerTest : public ::testing::Test { if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(position.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(position.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(position.at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(0), pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(position.at(1), pos_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(position.at(2), pos_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(velocity.at(0), joint_vel_[0]); - EXPECT_EQ(velocity.at(1), joint_vel_[1]); - EXPECT_EQ(velocity.at(2), joint_vel_[2]); + EXPECT_EQ(velocity.at(0), vel_cmd_interfaces_[0].get_value()); + EXPECT_EQ(velocity.at(1), vel_cmd_interfaces_[1].get_value()); + EXPECT_EQ(velocity.at(2), vel_cmd_interfaces_[2].get_value()); } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); + EXPECT_EQ(0.0, acc_cmd_interfaces_[0].get_value()); + EXPECT_EQ(0.0, acc_cmd_interfaces_[1].get_value()); + EXPECT_EQ(0.0, acc_cmd_interfaces_[2].get_value()); } if (traj_controller_->has_effort_command_interface()) { - EXPECT_EQ(0.0, joint_eff_[0]); - EXPECT_EQ(0.0, joint_eff_[1]); - EXPECT_EQ(0.0, joint_eff_[2]); + EXPECT_EQ(0.0, eff_cmd_interfaces_[0].get_value()); + EXPECT_EQ(0.0, eff_cmd_interfaces_[1].get_value()); + EXPECT_EQ(0.0, eff_cmd_interfaces_[2].get_value()); } } else // traj_controller_->use_closed_loop_pid_adapter() == true @@ -637,10 +637,11 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_value(), joint_vel_[i])) + position.at(i) - pos_state_interfaces_[i].get_value(), + vel_state_interfaces_[i].get_value())) << "test position point " << position.at(i) << ", position state is " << pos_state_interfaces_[i].get_value() << ", velocity command is " - << joint_vel_[i]; + << vel_state_interfaces_[i].get_value(); } } if (traj_controller_->has_effort_command_interface()) @@ -648,10 +649,11 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) + position.at(i) - pos_state_interfaces_[i].get_value(), + acc_state_interfaces_[i].get_value())) << "test position point " << position.at(i) << ", position state is " << pos_state_interfaces_[i].get_value() << ", effort command is " - << joint_eff_[i]; + << acc_state_interfaces_[i].get_value(); } } } @@ -664,30 +666,30 @@ class TrajectoryControllerTest : public ::testing::Test if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(point.at(0), pos_cmd_interfaces_[0].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(point.at(1), pos_cmd_interfaces_[1].get_value(), COMMON_THRESHOLD); + EXPECT_NEAR(point.at(2), pos_cmd_interfaces_[2].get_value(), COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(0.0, joint_vel_[0]); - EXPECT_EQ(0.0, joint_vel_[1]); - EXPECT_EQ(0.0, joint_vel_[2]); + EXPECT_EQ(0.0, vel_cmd_interfaces_[0].get_value()); + EXPECT_EQ(0.0, vel_cmd_interfaces_[1].get_value()); + EXPECT_EQ(0.0, vel_cmd_interfaces_[2].get_value()); } if (traj_controller_->has_acceleration_command_interface()) { - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); + EXPECT_EQ(0.0, acc_cmd_interfaces_[0].get_value()); + EXPECT_EQ(0.0, acc_cmd_interfaces_[1].get_value()); + EXPECT_EQ(0.0, acc_cmd_interfaces_[2].get_value()); } if (traj_controller_->has_effort_command_interface()) { - EXPECT_EQ(0.0, joint_eff_[0]); - EXPECT_EQ(0.0, joint_eff_[1]); - EXPECT_EQ(0.0, joint_eff_[2]); + EXPECT_EQ(0.0, eff_cmd_interfaces_[0].get_value()); + EXPECT_EQ(0.0, eff_cmd_interfaces_[1].get_value()); + EXPECT_EQ(0.0, eff_cmd_interfaces_[2].get_value()); } } @@ -748,13 +750,6 @@ class TrajectoryControllerTest : public ::testing::Test mutable std::mutex state_mutex_; std::shared_ptr state_msg_; - std::vector joint_pos_; - std::vector joint_vel_; - std::vector joint_acc_; - std::vector joint_eff_; - std::vector joint_state_pos_; - std::vector joint_state_vel_; - std::vector joint_state_acc_; std::vector pos_cmd_interfaces_; std::vector vel_cmd_interfaces_; std::vector acc_cmd_interfaces_; diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index cb27fadf3d..1894de95eb 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -104,9 +104,9 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl std::unique_ptr rt_odom_state_publisher_; std::unique_ptr rt_tf_odom_state_publisher_; + // override methods from ChainableControllerInterface std::vector export_state_interface_descriptions() override; - // override methods from ChainableControllerInterface std::vector export_reference_interface_descriptions() override; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index bd45d26b7e..17c2ddab25 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -50,7 +50,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto cmd_if_conf = controller_->command_interface_configuration(); - ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_size_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); @@ -62,7 +62,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); - ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_size_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); @@ -132,9 +132,9 @@ TEST_F(TricycleSteeringControllerTest, reactivate_success) 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); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -164,20 +164,22 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic) controller_interface::return_type::OK); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); - for (const auto & interface : controller_->reference_interfaces_) + EXPECT_EQ(controller_->ordered_reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(std::isnan(interface->get_value())); } } @@ -192,8 +194,8 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_TRUE(controller_->is_in_chained_mode()); - controller_->reference_interfaces_[0] = 0.1; - controller_->reference_interfaces_[1] = 0.2; + controller_->ordered_reference_interfaces_[0]->operator-=(0.1); + controller_->ordered_reference_interfaces_[1]->set_value(0.2); ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), @@ -201,20 +203,22 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained) // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); - EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); - for (const auto & interface : controller_->reference_interfaces_) + EXPECT_EQ(controller_->ordered_reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(std::isnan(interface->get_value())); } } @@ -247,13 +251,13 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224, - COMMON_THRESHOLD); + controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), + 0.22222222222222224, COMMON_THRESHOLD); EXPECT_NEAR( - controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, + controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734, COMMON_THRESHOLD); subscribe_and_get_messages(msg); diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 912c39b157..5a1685f9e2 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -146,39 +146,43 @@ class TricycleSteeringControllerFixture : public ::testing::Test } std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, - &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[0], + hardware_interface::InterfaceInfo(traction_interface_name_, "double", "1.1")))); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[1], steering_interface_name_, - &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[1], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "3.3")))); command_ifs.emplace_back(command_itfs_.back()); - command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + front_wheels_names_[0], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "2.2")))); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; - state_itfs_.reserve(joint_state_values_.size()); - state_ifs.reserve(joint_state_values_.size()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[0], + hardware_interface::InterfaceInfo(traction_interface_name_, "double", "0.5")))); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[1], traction_interface_name_, - &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + rear_wheels_names_[1], + hardware_interface::InterfaceInfo(traction_interface_name_, "double", "0.5")))); state_ifs.emplace_back(state_itfs_.back()); - state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + state_itfs_.emplace_back( + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + front_wheels_names_[0], + hardware_interface::InterfaceInfo(steering_interface_name_, "double", "0.0")))); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -278,8 +282,9 @@ class TricycleSteeringControllerFixture : public ::testing::Test double front_wheels_radius_ = 0.45; double rear_wheels_radius_ = 0.45; - std::array joint_state_values_ = {0.5, 0.5, 0.0}; - std::array joint_command_values_ = {1.1, 3.3, 2.2}; + const size_t joint_command_values_size_ = 3; + const size_t joint_state_values_size_ = 3; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; // defined in setup 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 2170659ee7..e91e214861 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -52,7 +52,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto cmd_if_conf = controller_->command_interface_configuration(); - ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); + ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_size_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); @@ -65,7 +65,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); - ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); + ASSERT_EQ(state_if_conf.names.size(), joint_state_values_size_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); @@ -78,7 +78,7 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs - auto ref_if_conf = controller_->export_reference_interfaces(); + auto ref_if_conf = controller_->export_reference_interface_descriptions(); ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size()); for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) {