diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d9d95bf8b5..4b955f063e 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -63,11 +63,13 @@ bool AckermannSteeringController::update_odometry(const rclcpp::Duration & perio else { const double traction_right_wheel_value = - state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); const double traction_left_wheel_value = - state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); - const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_right_position = + state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(); + const double steering_left_position = + state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(); if ( std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && std::isfinite(steering_right_position) && std::isfinite(steering_left_position)) 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..8c9d7dab5c 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; @@ -121,8 +124,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // internal reference values const std::vector allowed_reference_interfaces_types_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}; - std::vector> position_reference_; - std::vector> velocity_reference_; + std::vector position_reference_names_; + std::vector velocity_reference_names_; // Admittance rule and dependent variables; std::unique_ptr admittance_; diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e29b574a2..788a07c240 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -97,8 +97,14 @@ controller_interface::InterfaceConfiguration AdmittanceController::state_interfa controller_interface::interface_configuration_type::INDIVIDUAL, state_interfaces_config_names}; } -std::vector -AdmittanceController::on_export_reference_interfaces() +std::vector +AdmittanceController::export_state_interface_descriptions() +{ + return {}; +} + +std::vector +AdmittanceController::export_reference_interface_descriptions() { // create CommandInterface interfaces that other controllers will be able to chain with if (!admittance_) @@ -106,34 +112,32 @@ AdmittanceController::on_export_reference_interfaces() return {}; } - std::vector chainable_command_interfaces; + std::vector chainable_command_interfaces; const auto num_chainable_interfaces = admittance_->parameters_.chainable_command_interfaces.size() * admittance_->parameters_.joints.size(); // allocate dynamic memory chainable_command_interfaces.reserve(num_chainable_interfaces); - reference_interfaces_.resize(num_chainable_interfaces, std::numeric_limits::quiet_NaN()); - position_reference_ = {}; - velocity_reference_ = {}; + position_reference_names_ = {}; + velocity_reference_names_ = {}; - // assign reference interfaces - auto index = 0ul; + const auto prefix = std::string(get_node()->get_name()); + // create reference interfaces for (const auto & interface : allowed_reference_interfaces_types_) { for (const auto & joint : admittance_->parameters_.joints) { + const auto itf_name = joint + "/" + interface; + chainable_command_interfaces.emplace_back( + prefix, hardware_interface::InterfaceInfo(itf_name, "double")); + // add reference interface names in order to position or velocity if (hardware_interface::HW_IF_POSITION == interface) - position_reference_.emplace_back(reference_interfaces_[index]); + position_reference_names_.push_back(chainable_command_interfaces.back().get_name()); else if (hardware_interface::HW_IF_VELOCITY == interface) { - velocity_reference_.emplace_back(reference_interfaces_[index]); + velocity_reference_names_.push_back(chainable_command_interfaces.back().get_name()); } - const auto full_name = joint + "/" + interface; - chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface( - std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index)); - - index++; } } @@ -370,11 +374,13 @@ controller_interface::return_type AdmittanceController::update_reference_from_su { for (size_t i = 0; i < joint_command_msg_->positions.size(); ++i) { - position_reference_[i].get() = joint_command_msg_->positions[i]; + reference_interfaces_.at(position_reference_names_[i]) + ->set_value(joint_command_msg_->positions[i]); } for (size_t i = 0; i < joint_command_msg_->velocities.size(); ++i) { - velocity_reference_[i].get() = joint_command_msg_->velocities[i]; + reference_interfaces_.at(velocity_reference_names_[i]) + ->set_value(joint_command_msg_->velocities[i]); } } @@ -424,8 +430,10 @@ controller_interface::CallbackReturn AdmittanceController::on_deactivate( // reset to prevent stale references for (size_t i = 0; i < num_joints_; i++) { - position_reference_[i].get() = std::numeric_limits::quiet_NaN(); - velocity_reference_[i].get() = std::numeric_limits::quiet_NaN(); + reference_interfaces_.at(position_reference_names_[i]) + ->set_value(std::numeric_limits::quiet_NaN()); + reference_interfaces_.at(velocity_reference_names_[i]) + ->set_value(std::numeric_limits::quiet_NaN()); } for (size_t index = 0; index < allowed_interface_types_.size(); ++index) @@ -473,19 +481,19 @@ void AdmittanceController::read_state_from_hardware( if (has_position_state_interface_) { state_current.positions[joint_ind] = - state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); + state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value(); nan_position |= std::isnan(state_current.positions[joint_ind]); } else if (has_velocity_state_interface_) { state_current.velocities[joint_ind] = - state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); + state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value(); nan_velocity |= std::isnan(state_current.velocities[joint_ind]); } else if (has_acceleration_state_interface_) { state_current.accelerations[joint_ind] = - state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); + state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value(); nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]); } } @@ -551,18 +559,22 @@ void AdmittanceController::read_state_reference_interfaces( for (size_t i = 0; i < num_joints_; ++i) { // update position - if (std::isnan(position_reference_[i])) + if (std::isnan(reference_interfaces_.at(position_reference_names_[i])->get_value())) { - position_reference_[i].get() = last_reference_.positions[i]; + reference_interfaces_.at(position_reference_names_[i]) + ->set_value(last_reference_.positions[i]); } - state_reference.positions[i] = position_reference_[i]; + state_reference.positions[i] = + reference_interfaces_.at(position_reference_names_[i])->get_value(); // update velocity - if (std::isnan(velocity_reference_[i])) + if (std::isnan(reference_interfaces_.at(velocity_reference_names_[i])->get_value())) { - velocity_reference_[i].get() = last_reference_.velocities[i]; + reference_interfaces_.at(velocity_reference_names_[i]) + ->set_value(last_reference_.velocities[i]); } - state_reference.velocities[i] = velocity_reference_[i]; + state_reference.velocities[i] = + reference_interfaces_.at(velocity_reference_names_[i])->get_value(); } last_reference_.positions = state_reference.positions; diff --git a/admittance_controller/test/test_admittance_controller.cpp b/admittance_controller/test/test_admittance_controller.cpp index 5c4bbe9438..bda676e591 100644 --- a/admittance_controller/test/test_admittance_controller.cpp +++ b/admittance_controller/test/test_admittance_controller.cpp @@ -154,7 +154,7 @@ TEST_F(AdmittanceControllerTest, check_interfaces) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); auto command_interfaces = controller_->command_interface_configuration(); - ASSERT_EQ(command_interfaces.names.size(), joint_command_values_.size()); + ASSERT_EQ(command_interfaces.names.size(), initial_joint_command_values_.size()); EXPECT_EQ( command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); @@ -162,12 +162,14 @@ TEST_F(AdmittanceControllerTest, check_interfaces) controller_->command_interfaces_.size(), command_interface_types_.size() * joint_names_.size()); auto state_interfaces = controller_->state_interface_configuration(); - ASSERT_EQ(state_interfaces.names.size(), joint_state_values_.size() + fts_state_values_.size()); + ASSERT_EQ( + state_interfaces.names.size(), + initial_joint_state_values_.size() + initial_fts_state_values_.size()); EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); ASSERT_EQ( controller_->state_interfaces_.size(), - state_interface_types_.size() * joint_names_.size() + fts_state_values_.size()); + state_interface_types_.size() * joint_names_.size() + initial_fts_state_values_.size()); } TEST_F(AdmittanceControllerTest, activate_success) @@ -264,9 +266,10 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) controller_interface::return_type::OK); // After first update state, commanded position should be near the start state - for (auto i = 0ul; i < joint_state_values_.size(); i++) + for (auto i = 0ul; i < state_itfs_.size(); i++) { - ASSERT_NEAR(joint_state_values_[i], joint_command_values_[i], COMMON_THRESHOLD); + ASSERT_NEAR( + state_itfs_[i].get_value(), command_itfs_[i].get_value(), COMMON_THRESHOLD); } ControllerStateMsg msg; @@ -282,7 +285,8 @@ TEST_F(AdmittanceControllerTest, receive_message_and_publish_updated_status) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_NEAR(joint_command_values_[0], joint_state_values_[0], COMMON_THRESHOLD); + EXPECT_NEAR( + command_itfs_[0].get_value(), state_itfs_[0].get_value(), COMMON_THRESHOLD); subscribe_and_get_messages(msg); } diff --git a/admittance_controller/test/test_admittance_controller.hpp b/admittance_controller/test/test_admittance_controller.hpp index b2a95c12fa..8cfbeb86e7 100644 --- a/admittance_controller/test/test_admittance_controller.hpp +++ b/admittance_controller/test/test_admittance_controller.hpp @@ -174,13 +174,15 @@ class AdmittanceControllerTest : public ::testing::Test void assign_interfaces() { std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + command_itfs_.reserve(initial_joint_command_values_.size()); + command_ifs.reserve(initial_joint_command_values_.size()); - for (auto i = 0u; i < joint_command_values_.size(); ++i) + for (auto i = 0u; i < initial_joint_command_values_.size(); ++i) { - command_itfs_.emplace_back(hardware_interface::CommandInterface( - joint_names_[i], command_interface_types_[0], &joint_command_values_[i])); + command_itfs_.emplace_back(hardware_interface::InterfaceDescription( + joint_names_[i], hardware_interface::InterfaceInfo( + command_interface_types_[0], "double", + std::to_string(initial_joint_command_values_[i])))); command_ifs.emplace_back(command_itfs_.back()); } @@ -188,14 +190,16 @@ class AdmittanceControllerTest : public ::testing::Test fts_state_names_ = sc_fts.get_state_interface_names(); std::vector state_ifs; - const size_t num_state_ifs = joint_state_values_.size() + fts_state_names_.size(); + const size_t num_state_ifs = initial_joint_state_values_.size() + fts_state_names_.size(); state_itfs_.reserve(num_state_ifs); state_ifs.reserve(num_state_ifs); - for (auto i = 0u; i < joint_state_values_.size(); ++i) + for (auto i = 0u; i < initial_joint_state_values_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( - joint_names_[i], state_interface_types_[0], &joint_state_values_[i])); + state_itfs_.emplace_back(hardware_interface::InterfaceDescription( + joint_names_[i], + hardware_interface::InterfaceInfo( + state_interface_types_[0], std::to_string(initial_joint_command_values_[i])))); state_ifs.emplace_back(state_itfs_.back()); } @@ -204,8 +208,10 @@ class AdmittanceControllerTest : public ::testing::Test for (auto i = 0u; i < fts_state_names_.size(); ++i) { - state_itfs_.emplace_back(hardware_interface::StateInterface( - ft_sensor_name_, fts_itf_names[i], &fts_state_values_[i])); + state_itfs_.emplace_back(hardware_interface::InterfaceDescription( + ft_sensor_name_, + hardware_interface::InterfaceInfo( + fts_itf_names[i], "double", std::to_string(initial_fts_state_values_[i])))); state_ifs.emplace_back(state_itfs_.back()); } @@ -344,7 +350,7 @@ class AdmittanceControllerTest : public ::testing::Test trajectory_point.velocities.resize(num_joints, 0.0); for (auto index = 0u; index < num_joints; ++index) { - trajectory_point.positions.emplace_back(joint_state_values_[index]); + trajectory_point.positions.emplace_back(initial_joint_state_values_[index]); } joint_msg = trajectory_point; @@ -380,9 +386,9 @@ class AdmittanceControllerTest : public ::testing::Test 2.828427, 2.828427, 2.828427}; std::array admittance_stiffness_ = {214.1, 214.2, 214.3, 214.4, 214.5, 214.6}; - std::array joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - std::array joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; - std::array fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + const std::array initial_joint_command_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + const std::array initial_joint_state_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; + const std::array initial_fts_state_values_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; std::vector fts_state_names_; std::vector state_itfs_; diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 95eaf1965c..1e4002cb5b 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -60,8 +60,8 @@ bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period) } else { - const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); - const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position)) { if (params_.position_feedback) 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/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a64f210503..de0e05692e 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -151,9 +151,10 @@ controller_interface::return_type DiffDriveController::update( double right_feedback_mean = 0.0; for (size_t index = 0; index < static_cast(wheels_per_side_); ++index) { - const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value(); + const double left_feedback = + registered_left_wheel_handles_[index].feedback.get().get_value(); const double right_feedback = - registered_right_wheel_handles_[index].feedback.get().get_value(); + registered_right_wheel_handles_[index].feedback.get().get_value(); if (std::isnan(left_feedback) || std::isnan(right_feedback)) { diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 72ae4dbfd7..6bff7cb5b8 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -21,6 +21,7 @@ #include #include "diff_drive_controller/diff_drive_controller.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -31,6 +32,8 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; @@ -197,17 +200,17 @@ class TestDiffDriveController : public ::testing::Test std::vector velocity_values_ = {0.01, 0.02}; hardware_interface::StateInterface left_wheel_pos_state_{ - left_wheel_names[0], HW_IF_POSITION, &position_values_[0]}; + InterfaceDescription(left_wheel_names[0], InterfaceInfo(HW_IF_POSITION, "0.1", "double"))}; hardware_interface::StateInterface right_wheel_pos_state_{ - right_wheel_names[0], HW_IF_POSITION, &position_values_[1]}; + InterfaceDescription(right_wheel_names[0], InterfaceInfo(HW_IF_POSITION, "0.2", "double"))}; hardware_interface::StateInterface left_wheel_vel_state_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + InterfaceDescription(left_wheel_names[0], InterfaceInfo(HW_IF_VELOCITY, "0.01", "double"))}; hardware_interface::StateInterface right_wheel_vel_state_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + InterfaceDescription(right_wheel_names[0], InterfaceInfo(HW_IF_VELOCITY, "0.02", "double"))}; hardware_interface::CommandInterface left_wheel_vel_cmd_{ - left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]}; + InterfaceDescription(left_wheel_names[0], InterfaceInfo(HW_IF_VELOCITY, "0.01", "double"))}; hardware_interface::CommandInterface right_wheel_vel_cmd_{ - right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]}; + InterfaceDescription(right_wheel_names[0], InterfaceInfo(HW_IF_VELOCITY, "0.02", "double"))}; rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -513,8 +516,8 @@ TEST_F(TestDiffDriveController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); executor.cancel(); } @@ -534,8 +537,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) assignResourcesPosFeedback(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.02, right_wheel_vel_cmd_.get_value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -550,8 +553,8 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(1.0, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(1.0, right_wheel_vel_cmd_.get_value()); // deactivated // wait so controller process the second point when deactivated @@ -562,14 +565,14 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); - EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()); + EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp index c19cbff9cf..0dee050c6a 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.cpp +++ b/effort_controllers/test/test_joint_group_effort_controller.cpp @@ -105,9 +105,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -120,9 +120,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); } TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) @@ -142,9 +142,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); } TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) @@ -159,9 +159,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); } TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) @@ -170,9 +170,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -203,9 +203,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); } TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) @@ -217,15 +217,15 @@ TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); } diff --git a/effort_controllers/test/test_joint_group_effort_controller.hpp b/effort_controllers/test/test_joint_group_effort_controller.hpp index 5af880d792..be052b57b9 100644 --- a/effort_controllers/test/test_joint_group_effort_controller.hpp +++ b/effort_controllers/test/test_joint_group_effort_controller.hpp @@ -23,11 +23,14 @@ #include "effort_controllers/joint_group_effort_controller.hpp" #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // subclassing and friending so we can access member variables class FriendJointGroupEffortController : public effort_controllers::JointGroupEffortController { @@ -53,11 +56,13 @@ class JointGroupEffortControllerTest : public ::testing::Test // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; - std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]}; - CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]}; - CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]}; + CommandInterface joint_1_cmd_{ + InterfaceDescription(joint_names_[0], InterfaceInfo(HW_IF_EFFORT, "1.1", "double"))}; + CommandInterface joint_2_cmd_{ + InterfaceDescription(joint_names_[1], InterfaceInfo(HW_IF_EFFORT, "2.1", "double"))}; + CommandInterface joint_3_cmd_{ + InterfaceDescription(joint_names_[2], InterfaceInfo(HW_IF_EFFORT, "3.1", "double"))}; rclcpp::executors::SingleThreadedExecutor executor; }; diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp index fe7fb0c174..ccf00d297c 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp @@ -57,12 +57,24 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test const std::string frame_id_ = "fts_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6}; - hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]}; - hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]}; - hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]}; - hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]}; - hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]}; - hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]}; + hardware_interface::StateInterface fts_force_x_{hardware_interface::InterfaceDescription( + sensor_name_, + hardware_interface::InterfaceInfo("force.x", "double", std::to_string(sensor_values_[0])))}; + hardware_interface::StateInterface fts_force_y_{hardware_interface::InterfaceDescription( + sensor_name_, + hardware_interface::InterfaceInfo("force.y", "double", std::to_string(sensor_values_[1])))}; + hardware_interface::StateInterface fts_force_z_{hardware_interface::InterfaceDescription( + sensor_name_, + hardware_interface::InterfaceInfo("force.z", "double", std::to_string(sensor_values_[2])))}; + hardware_interface::StateInterface fts_torque_x_{hardware_interface::InterfaceDescription( + sensor_name_, + hardware_interface::InterfaceInfo("torque.x", "double", std::to_string(sensor_values_[3])))}; + hardware_interface::StateInterface fts_torque_y_{hardware_interface::InterfaceDescription( + sensor_name_, + hardware_interface::InterfaceInfo("torque.y", "double", std::to_string(sensor_values_[4])))}; + hardware_interface::StateInterface fts_torque_z_{hardware_interface::InterfaceDescription( + sensor_name_, + hardware_interface::InterfaceInfo("torque.z", "double", std::to_string(sensor_values_[5])))}; std::unique_ptr fts_broadcaster_; diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp index fcfa65ee1c..0cea7cd383 100644 --- a/forward_command_controller/test/test_forward_command_controller.cpp +++ b/forward_command_controller/test/test_forward_command_controller.cpp @@ -212,9 +212,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -227,9 +227,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); } TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) @@ -255,9 +255,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); } TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) @@ -277,9 +277,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); } TEST_F(ForwardCommandControllerTest, CommandCallbackTest) @@ -290,9 +290,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -323,9 +323,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); } TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -336,9 +336,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_->get_node()->set_parameter({"interface_name", "position"}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); auto node_state = controller_->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -373,9 +373,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -425,9 +425,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -438,7 +438,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); } diff --git a/forward_command_controller/test/test_forward_command_controller.hpp b/forward_command_controller/test/test_forward_command_controller.hpp index 2284d46d61..14c37200d1 100644 --- a/forward_command_controller/test/test_forward_command_controller.hpp +++ b/forward_command_controller/test/test_forward_command_controller.hpp @@ -23,11 +23,14 @@ #include "forward_command_controller/forward_command_controller.hpp" #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // subclassing and friending so we can access member variables class FriendForwardCommandController : public forward_command_controller::ForwardCommandController @@ -65,11 +68,13 @@ class ForwardCommandControllerTest : public ::testing::Test // dummy joint state values used for tests const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; - std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; - CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; - CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + CommandInterface joint_1_pos_cmd_{ + InterfaceDescription(joint_names_[0], InterfaceInfo(HW_IF_POSITION, "1.1", "double"))}; + CommandInterface joint_2_pos_cmd_{ + InterfaceDescription(joint_names_[1], InterfaceInfo(HW_IF_POSITION, "1.2", "double"))}; + CommandInterface joint_3_pos_cmd_{ + InterfaceDescription(joint_names_[2], InterfaceInfo(HW_IF_POSITION, "3.1", "double"))}; rclcpp::executors::SingleThreadedExecutor executor; }; diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp index c52f4f3ab6..d9fd41d95d 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp @@ -189,9 +189,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess) SetUpController(true); // check joint commands are the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) @@ -209,9 +209,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) @@ -224,9 +224,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) @@ -244,9 +244,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1); } TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) @@ -276,9 +276,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); } TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess) @@ -304,9 +304,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); auto node_state = controller_->get_node()->deactivate(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -355,9 +355,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // values should not change - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0); // set commands again controller_->rt_command_ptr_.writeFromNonRT(command_msg); @@ -368,7 +368,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); - ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6); - ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); + ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6); + ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7); } diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp index 78b244847b..66bb803c34 100644 --- a/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp +++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.hpp @@ -25,6 +25,7 @@ #include "forward_command_controller/multi_interface_forward_command_controller.hpp" #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" @@ -32,6 +33,8 @@ using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // subclassing and friending so we can access member variables class FriendMultiInterfaceForwardCommandController @@ -71,13 +74,12 @@ class MultiInterfaceForwardCommandControllerTest : public ::testing::Test // dummy joint state value used for tests const std::string joint_name_ = "joint1"; - double pos_cmd_ = 1.1; - double vel_cmd_ = 2.1; - double eff_cmd_ = 3.1; - - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_}; - CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_}; - CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_}; + CommandInterface joint_1_pos_cmd_{ + InterfaceDescription(joint_name_, InterfaceInfo(HW_IF_POSITION, "1.1", "double"))}; + CommandInterface joint_1_vel_cmd_{ + InterfaceDescription(joint_name_, InterfaceInfo(HW_IF_VELOCITY, "2.1", "double"))}; + CommandInterface joint_1_eff_cmd_{ + InterfaceDescription(joint_name_, InterfaceInfo(HW_IF_EFFORT, "3.1", "double"))}; rclcpp::executors::SingleThreadedExecutor executor; }; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 2c115091af..09bdd5236f 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -65,8 +65,8 @@ controller_interface::return_type GripperActionController::up { command_struct_rt_ = *(command_.readFromRT()); - const double current_position = joint_position_state_interface_->get().get_value(); - const double current_velocity = joint_velocity_state_interface_->get().get_value(); + const double current_position = joint_position_state_interface_->get().get_value(); + const double current_velocity = joint_velocity_state_interface_->get().get_value(); const double error_position = command_struct_rt_.position_ - current_position; const double error_velocity = -current_velocity; @@ -147,7 +147,7 @@ rclcpp_action::CancelResponse GripperActionController::cancel template void GripperActionController::set_hold_position() { - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; command_.writeFromNonRT(command_struct_); } @@ -292,7 +292,7 @@ controller_interface::CallbackReturn GripperActionController: hw_iface_adapter_.init(joint_command_interface_, get_node()); // Command - non RT version - command_struct_.position_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; command_.initRT(command_struct_); diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index cb92bc9dcd..f2ca717e03 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -23,12 +23,15 @@ #include "gripper_controllers/gripper_action_controller.hpp" #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::StateInterface; namespace @@ -59,12 +62,13 @@ class GripperControllerTest : public ::testing::Test // dummy joint state values used for tests const std::string joint_name_ = "joint1"; - std::vector joint_states_ = {1.1, 2.1}; - std::vector joint_commands_ = {3.1}; - StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; - StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; - CommandInterface joint_1_cmd_{joint_name_, T::value, &joint_commands_[0]}; + StateInterface joint_1_pos_state_{ + InterfaceDescription(joint_name_, InterfaceInfo(HW_IF_POSITION, "1.1", "double"))}; + StateInterface joint_1_vel_state_{ + InterfaceDescription(joint_name_, InterfaceInfo(HW_IF_VELOCITY, "2.1", "double"))}; + CommandInterface joint_1_cmd_{ + InterfaceDescription(joint_name_, InterfaceInfo(T::value, "3.1", "double"))}; }; } // anonymous namespace diff --git a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp index 0f3286c302..7d7d6c7dad 100644 --- a/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp +++ b/imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp @@ -24,8 +24,11 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp" +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // subclassing and friending so we can access member variables class FriendIMUSensorBroadcaster : public imu_sensor_broadcaster::IMUSensorBroadcaster { @@ -56,25 +59,25 @@ class IMUSensorBroadcasterTest : public ::testing::Test const std::string frame_id_ = "imu_sensor_frame"; std::array sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10}; hardware_interface::StateInterface imu_orientation_x_{ - sensor_name_, "orientation.x", &sensor_values_[0]}; + InterfaceDescription(sensor_name_, InterfaceInfo("orientation.x", "1.1", "double"))}; hardware_interface::StateInterface imu_orientation_y_{ - sensor_name_, "orientation.y", &sensor_values_[1]}; + InterfaceDescription(sensor_name_, InterfaceInfo("orientation.y", "2.2", "double"))}; hardware_interface::StateInterface imu_orientation_z_{ - sensor_name_, "orientation.z", &sensor_values_[2]}; + InterfaceDescription(sensor_name_, InterfaceInfo("orientation.z", "3.3", "double"))}; hardware_interface::StateInterface imu_orientation_w_{ - sensor_name_, "orientation.w", &sensor_values_[3]}; + InterfaceDescription(sensor_name_, InterfaceInfo("orientation.w", "4.4", "double"))}; hardware_interface::StateInterface imu_angular_velocity_x_{ - sensor_name_, "angular_velocity.x", &sensor_values_[4]}; + InterfaceDescription(sensor_name_, InterfaceInfo("angular_velocity.x", "5.5", "double"))}; hardware_interface::StateInterface imu_angular_velocity_y_{ - sensor_name_, "angular_velocity.y", &sensor_values_[5]}; + InterfaceDescription(sensor_name_, InterfaceInfo("angular_velocity.y", "6.6", "double"))}; hardware_interface::StateInterface imu_angular_velocity_z_{ - sensor_name_, "angular_velocity.z", &sensor_values_[6]}; + InterfaceDescription(sensor_name_, InterfaceInfo("angular_velocity.z", "7.7", "double"))}; hardware_interface::StateInterface imu_linear_acceleration_x_{ - sensor_name_, "linear_acceleration.x", &sensor_values_[7]}; + InterfaceDescription(sensor_name_, InterfaceInfo("linear_acceleration.x", "8.8", "double"))}; hardware_interface::StateInterface imu_linear_acceleration_y_{ - sensor_name_, "linear_acceleration.y", &sensor_values_[8]}; + InterfaceDescription(sensor_name_, InterfaceInfo("linear_acceleration.y", "9.9", "double"))}; hardware_interface::StateInterface imu_linear_acceleration_z_{ - sensor_name_, "linear_acceleration.z", &sensor_values_[9]}; + InterfaceDescription(sensor_name_, InterfaceInfo("linear_acceleration.z", "10.10", "double"))}; std::unique_ptr imu_broadcaster_; diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5eb0a9b9b6..00e0c5a5b9 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -338,10 +338,10 @@ controller_interface::return_type JointStateBroadcaster::update( interface_name = map_interface_to_joint_state_[interface_name]; } name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] = - state_interface.get_value(); + state_interface.get_value(); RCLCPP_DEBUG( get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(), - state_interface.get_value()); + state_interface.get_value()); } if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock()) diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 3e54116d5c..11b5941f14 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -23,6 +23,7 @@ #include "joint_state_broadcaster/joint_state_broadcaster.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" using hardware_interface::HW_IF_EFFORT; @@ -82,27 +83,37 @@ class JointStateBroadcasterTest : public ::testing::Test std::vector joint_values_ = {1.1, 2.1, 3.1}; double custom_joint_value_ = 3.5; - hardware_interface::StateInterface joint_1_pos_state_{ - joint_names_[0], interface_names_[0], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_pos_state_{ - joint_names_[1], interface_names_[0], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_pos_state_{ - joint_names_[2], interface_names_[0], &joint_values_[2]}; - hardware_interface::StateInterface joint_1_vel_state_{ - joint_names_[0], interface_names_[1], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_vel_state_{ - joint_names_[1], interface_names_[1], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_vel_state_{ - joint_names_[2], interface_names_[1], &joint_values_[2]}; - hardware_interface::StateInterface joint_1_eff_state_{ - joint_names_[0], interface_names_[2], &joint_values_[0]}; - hardware_interface::StateInterface joint_2_eff_state_{ - joint_names_[1], interface_names_[2], &joint_values_[1]}; - hardware_interface::StateInterface joint_3_eff_state_{ - joint_names_[2], interface_names_[2], &joint_values_[2]}; - - hardware_interface::StateInterface joint_X_custom_state{ - joint_names_[0], custom_interface_name_, &custom_joint_value_}; + hardware_interface::StateInterface joint_1_pos_state_{hardware_interface::InterfaceDescription( + joint_names_[0], hardware_interface::InterfaceInfo( + interface_names_[0], "double", std::to_string(joint_values_[0])))}; + hardware_interface::StateInterface joint_2_pos_state_{hardware_interface::InterfaceDescription( + joint_names_[1], hardware_interface::InterfaceInfo( + interface_names_[0], "double", std::to_string(joint_values_[1])))}; + hardware_interface::StateInterface joint_3_pos_state_{hardware_interface::InterfaceDescription( + joint_names_[2], hardware_interface::InterfaceInfo( + interface_names_[0], "double", std::to_string(joint_values_[2])))}; + hardware_interface::StateInterface joint_1_vel_state_{hardware_interface::InterfaceDescription( + joint_names_[0], hardware_interface::InterfaceInfo( + interface_names_[1], "double", std::to_string(joint_values_[0])))}; + hardware_interface::StateInterface joint_2_vel_state_{hardware_interface::InterfaceDescription( + joint_names_[1], hardware_interface::InterfaceInfo( + interface_names_[1], "double", std::to_string(joint_values_[1])))}; + hardware_interface::StateInterface joint_3_vel_state_{hardware_interface::InterfaceDescription( + joint_names_[2], hardware_interface::InterfaceInfo( + interface_names_[1], "double", std::to_string(joint_values_[2])))}; + hardware_interface::StateInterface joint_1_eff_state_{hardware_interface::InterfaceDescription( + joint_names_[0], hardware_interface::InterfaceInfo( + interface_names_[2], "double", std::to_string(joint_values_[0])))}; + hardware_interface::StateInterface joint_2_eff_state_{hardware_interface::InterfaceDescription( + joint_names_[1], hardware_interface::InterfaceInfo( + interface_names_[2], "double", std::to_string(joint_values_[1])))}; + hardware_interface::StateInterface joint_3_eff_state_{hardware_interface::InterfaceDescription( + joint_names_[2], hardware_interface::InterfaceInfo( + interface_names_[2], "double", std::to_string(joint_values_[2])))}; + + hardware_interface::StateInterface joint_X_custom_state{hardware_interface::InterfaceDescription( + joint_names_[0], hardware_interface::InterfaceInfo( + custom_interface_name_, "double", std::to_string(custom_joint_value_)))}; std::unique_ptr state_broadcaster_; }; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9563568ad5..70c3344bcd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -410,11 +410,14 @@ 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) { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); + trajectory_point_interface[index] = joint_interface[index].get().get_value(); } }; @@ -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. @@ -983,7 +1000,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( if (has_position_command_interface_) { joint_command_interface_[0][index].get().set_value( - joint_command_interface_[0][index].get().get_value()); + joint_command_interface_[0][index].get().get_value()); } if (has_velocity_command_interface_) @@ -1248,15 +1265,15 @@ void JointTrajectoryController::fill_partial_goal( { if ( has_position_command_interface_ && - !std::isnan(joint_command_interface_[0][index].get().get_value())) + !std::isnan(joint_command_interface_[0][index].get().get_value())) { // copy last command if cmd interface exists - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + it.positions.push_back(joint_command_interface_[0][index].get().get_value()); } else if (has_position_state_interface_) { // copy current state if state interface exists - it.positions.push_back(joint_state_interface_[0][index].get().get_value()); + it.positions.push_back(joint_state_interface_[0][index].get().get_value()); } } if (!it.velocities.empty()) 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 796503c036..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,9 +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]; + << pos_state_interfaces_[i].get_value() << ", velocity command is " + << vel_state_interfaces_[i].get_value(); } } if (traj_controller_->has_effort_command_interface()) @@ -647,9 +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]; + << pos_state_interfaces_[i].get_value() << ", effort command is " + << acc_state_interfaces_[i].get_value(); } } } @@ -662,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()); } } @@ -746,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/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp index d37cb87fc8..f67933d15d 100644 --- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp +++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp @@ -59,8 +59,8 @@ controller_interface::return_type GripperActionController::update( { command_struct_rt_ = *(command_.readFromRT()); - const double current_position = joint_position_state_interface_->get().get_value(); - const double current_velocity = joint_velocity_state_interface_->get().get_value(); + const double current_position = joint_position_state_interface_->get().get_value(); + const double current_velocity = joint_velocity_state_interface_->get().get_value(); const double error_position = command_struct_rt_.position_cmd_ - current_position; check_for_success(get_node()->now(), error_position, current_position, current_velocity); @@ -168,7 +168,7 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback( void GripperActionController::set_hold_position() { - command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; command_.writeFromNonRT(command_struct_); @@ -323,7 +323,7 @@ controller_interface::CallbackReturn GripperActionController::on_activate( } // Command - non RT version - command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); + command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value(); command_struct_.max_effort_ = params_.max_effort; command_struct_.max_velocity_ = params_.max_velocity; command_.initRT(command_struct_); diff --git a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp index d206097782..9ab03d8946 100644 --- a/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp +++ b/parallel_gripper_controller/test/test_parallel_gripper_controller.hpp @@ -51,15 +51,16 @@ class GripperControllerTest : public ::testing::Test // dummy joint state values used for tests const std::string joint_name_ = "joint1"; - std::vector joint_states_ = {1.1, 2.1}; - std::vector joint_commands_ = {3.1}; - hardware_interface::StateInterface joint_1_pos_state_{ - joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]}; - hardware_interface::StateInterface joint_1_vel_state_{ - joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]}; - hardware_interface::CommandInterface joint_1_cmd_{ - joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]}; + hardware_interface::StateInterface joint_1_pos_state_{hardware_interface::InterfaceDescription( + joint_name_, + hardware_interface::InterfaceInfo(hardware_interface::HW_IF_POSITION, "double", "1.1"))}; + hardware_interface::StateInterface joint_1_vel_state_{hardware_interface::InterfaceDescription( + joint_name_, + hardware_interface::InterfaceInfo(hardware_interface::HW_IF_VELOCITY, "double", "2.1"))}; + hardware_interface::CommandInterface joint_1_cmd_{hardware_interface::InterfaceDescription( + joint_name_, + hardware_interface::InterfaceInfo(hardware_interface::HW_IF_POSITION, "double", "3.1"))}; }; } // anonymous namespace diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 236b067929..5e6006ce5f 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include "control_msgs/msg/multi_dof_command.hpp" @@ -91,8 +92,11 @@ class PidController : public controller_interface::ChainableControllerInterface pid_controller::Params params_; std::vector reference_and_state_dof_names_; - size_t dof_; - std::vector measured_state_values_; + std::string itf_; + std::string itf_dot_; + std::string node_name; + std::vector dof_names_; + std::unordered_map measured_state_values_; using PidPtr = std::shared_ptr; std::vector pids_; @@ -114,8 +118,12 @@ class PidController : public controller_interface::ChainableControllerInterface rclcpp::Publisher::SharedPtr s_publisher_; std::unique_ptr state_publisher_; + std::vector export_state_interface_descriptions() + override; + // override methods from ChainableControllerInterface - std::vector on_export_reference_interfaces() override; + std::vector export_reference_interface_descriptions() + override; bool on_set_chained_mode(bool chained_mode) override; diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index c8e6cc0fe0..1aecaf2855 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -24,6 +24,10 @@ #include "angles/angles.h" #include "control_msgs/msg/single_dof_state.hpp" +#include "controller_interface/helpers.hpp" +#include "hardware_interface/hardware_info.hpp" + +#include "rclcpp/rclcpp.hpp" #include "rclcpp/version.h" namespace @@ -50,7 +54,8 @@ static const rmw_qos_profile_t qos_services = { #endif using ControllerCommandMsg = pid_controller::PidController::ControllerReferenceMsg; - +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // called from RT control loop void reset_controller_reference_msg( const std::shared_ptr & msg, const std::vector & dof_names) @@ -121,21 +126,21 @@ controller_interface::CallbackReturn PidController::configure_parameters() return CallbackReturn::FAILURE; } - dof_ = params_.dof_names.size(); + dof_names_ = params_.dof_names; // TODO(destogl): is this even possible? Test it... - if (params_.gains.dof_names_map.size() != dof_) + if (params_.gains.dof_names_map.size() != dof_names_.size()) { RCLCPP_FATAL( get_node()->get_logger(), "Size of 'gains' (%zu) map and number or 'dof_names' (%zu) have to be the same!", - params_.gains.dof_names_map.size(), dof_); + params_.gains.dof_names_map.size(), dof_names_.size()); return CallbackReturn::FAILURE; } - pids_.resize(dof_); + pids_.resize(dof_names_.size()); - for (size_t i = 0; i < dof_; ++i) + for (size_t i = 0; i < dof_names_.size(); ++i) { // prefix should be interpreted as parameters prefix pids_[i] = @@ -198,8 +203,13 @@ controller_interface::CallbackReturn PidController::on_configure( reset_controller_measured_state_msg(measured_state_msg, reference_and_state_dof_names_); measured_state_.writeFromNonRT(measured_state_msg); - measured_state_values_.resize( - dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + for (const auto & dof_name : params_.reference_and_state_dof_names) + { + for (const auto & itf_name : params_.reference_and_state_interfaces) + { + measured_state_values_[dof_name + "/" + itf_name] = std::numeric_limits::quiet_NaN(); + } + } auto set_feedforward_control_callback = [&]( @@ -235,6 +245,25 @@ controller_interface::CallbackReturn PidController::on_configure( return controller_interface::CallbackReturn::ERROR; } + if ( + params_.reference_and_state_interfaces.size() == 0 || + params_.reference_and_state_interfaces[0].empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "No reference_and_state_interface name given. Need to give at least one."); + return controller_interface::CallbackReturn::ERROR; + } + itf_ = "/" + params_.reference_and_state_interfaces[0]; + + if ( + params_.reference_and_state_interfaces.size() == 2 && + !params_.reference_and_state_interfaces[1].empty()) + { + itf_dot_ = "/" + params_.reference_and_state_interfaces[1]; + } + node_name = std::string(get_node()->get_name()) + "/"; + // Reserve memory in state publisher state_publisher_->lock(); state_publisher_->msg_.dof_states.resize(reference_and_state_dof_names_.size()); @@ -328,7 +357,8 @@ controller_interface::InterfaceConfiguration PidController::state_interface_conf { state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(dof_ * params_.reference_and_state_interfaces.size()); + state_interfaces_config.names.reserve( + dof_names_.size() * params_.reference_and_state_interfaces.size()); for (const auto & interface : params_.reference_and_state_interfaces) { for (const auto & dof_name : reference_and_state_dof_names_) @@ -341,26 +371,32 @@ controller_interface::InterfaceConfiguration PidController::state_interface_conf return state_interfaces_config; } -std::vector PidController::on_export_reference_interfaces() +std::vector +PidController::export_state_interface_descriptions() +{ + // does not export any StateInterfaces + return {}; +} + +std::vector +PidController::export_reference_interface_descriptions() { - reference_interfaces_.resize( - dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits::quiet_NaN()); + reference_interfaces_.reserve(dof_names_.size() * params_.reference_and_state_interfaces.size()); - std::vector reference_interfaces; - reference_interfaces.reserve(reference_interfaces_.size()); + std::vector reference_interfaces_descr; + reference_interfaces_descr.reserve(reference_interfaces_.size()); - size_t index = 0; for (const auto & interface : params_.reference_and_state_interfaces) { for (const auto & dof_name : reference_and_state_dof_names_) { - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index])); - ++index; + reference_interfaces_descr.push_back(hardware_interface::InterfaceDescription( + get_node()->get_name(), + hardware_interface::InterfaceInfo(dof_name + "/" + interface, std::string("double")))); } } - return reference_interfaces; + return reference_interfaces_descr; } bool PidController::on_set_chained_mode(bool chained_mode) @@ -377,10 +413,15 @@ controller_interface::CallbackReturn PidController::on_activate( reset_controller_measured_state_msg( *(measured_state_.readFromRT()), reference_and_state_dof_names_); - reference_interfaces_.assign( - reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); - measured_state_values_.assign( - measured_state_values_.size(), std::numeric_limits::quiet_NaN()); + for (auto & [name, interface] : reference_interfaces_) + { + interface->set_value(std::numeric_limits::quiet_NaN()); + } + + for (const auto & [name, value] : measured_state_values_) + { + measured_state_values_[name] = std::numeric_limits::quiet_NaN(); + } return controller_interface::CallbackReturn::SUCCESS; } @@ -394,19 +435,23 @@ controller_interface::CallbackReturn PidController::on_deactivate( controller_interface::return_type PidController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto current_ref = input_ref_.readFromRT(); + auto current_ref = *input_ref_.readFromRT(); - for (size_t i = 0; i < dof_; ++i) + for (size_t i = 0; i < reference_and_state_dof_names_.size(); ++i) { - if (!std::isnan((*current_ref)->values[i])) + if (!std::isnan(current_ref->values[i])) { - reference_interfaces_[i] = (*current_ref)->values[i]; - if (reference_interfaces_.size() == 2 * dof_ && !std::isnan((*current_ref)->values_dot[i])) + const auto ref_itf = node_name + current_ref->dof_names[i] + itf_; + reference_interfaces_[ref_itf]->set_value(current_ref->values[i]); + if ( + reference_interfaces_.size() == 2 * dof_names_.size() && !itf_dot_.empty() && + !std::isnan(current_ref->values_dot[i])) { - reference_interfaces_[dof_ + i] = (*current_ref)->values_dot[i]; + const auto ref_itf_dot = node_name + current_ref->dof_names[i] + itf_dot_; + reference_interfaces_[ref_itf_dot]->set_value(current_ref->values_dot[i]); } - (*current_ref)->values[i] = std::numeric_limits::quiet_NaN(); + current_ref->values[i] = std::numeric_limits::quiet_NaN(); } } return controller_interface::return_type::OK; @@ -421,12 +466,16 @@ controller_interface::return_type PidController::update_and_write_commands( if (params_.use_external_measured_states) { const auto measured_state = *(measured_state_.readFromRT()); - for (size_t i = 0; i < dof_; ++i) + for (size_t i = 0; i < reference_and_state_dof_names_.size(); ++i) { - measured_state_values_[i] = measured_state->values[i]; - if (measured_state_values_.size() == 2 * dof_) + const auto itf_name = measured_state->dof_names[i] + itf_; + measured_state_values_[itf_name] = measured_state->values[i]; + if ( + measured_state_values_.size() == 2 * dof_names_.size() && !itf_dot_.empty() && + !std::isnan(measured_state->values_dot[i])) { - measured_state_values_[dof_ + i] = measured_state->values_dot[i]; + const auto itf_dot_name = measured_state->dof_names[i] + itf_dot_; + measured_state_values_[itf_dot_name] = measured_state->values_dot[i]; } } } @@ -434,21 +483,27 @@ controller_interface::return_type PidController::update_and_write_commands( { for (size_t i = 0; i < measured_state_values_.size(); ++i) { - measured_state_values_[i] = state_interfaces_[i].get_value(); + measured_state_values_[state_interfaces_[i].get_name()] = + state_interfaces_[i].get_value(); } } - for (size_t i = 0; i < dof_; ++i) + for (size_t i = 0; i < dof_names_.size(); ++i) { double tmp_command = std::numeric_limits::quiet_NaN(); - + const auto itf = reference_and_state_dof_names_[i] + itf_; + const auto itf_dot = reference_and_state_dof_names_[i] + itf_dot_; + const auto ref_itf = node_name + itf; + const auto ref_itf_dot = node_name + itf_dot; // Using feedforward - if (!std::isnan(reference_interfaces_[i]) && !std::isnan(measured_state_values_[i])) + if ( + !std::isnan(reference_interfaces_[ref_itf]->get_value()) && + !std::isnan(measured_state_values_[itf])) { // calculate feed-forward if (*(control_mode_.readFromRT()) == feedforward_mode_type::ON) { - tmp_command = reference_interfaces_[dof_ + i] * + tmp_command = reference_interfaces_[ref_itf_dot]->get_value() * params_.gains.dof_names_map[params_.dof_names[i]].feedforward_gain; } else @@ -456,24 +511,30 @@ controller_interface::return_type PidController::update_and_write_commands( tmp_command = 0.0; } - double error = reference_interfaces_[i] - measured_state_values_[i]; + double error = + reference_interfaces_[ref_itf]->get_value() - measured_state_values_[itf]; if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) { // for continuous angles the error is normalized between -piget_value()); } // checking if there are two interfaces - if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + if ( + reference_interfaces_.size() == 2 * dof_names_.size() && + measured_state_values_.size() == 2 * dof_names_.size()) { if ( - !std::isnan(reference_interfaces_[dof_ + i]) && - !std::isnan(measured_state_values_[dof_ + i])) + !std::isnan(reference_interfaces_[ref_itf_dot]->get_value()) && + !std::isnan(measured_state_values_[itf_dot])) { // use calculation with 'error' and 'error_dot' tmp_command += pids_[i]->computeCommand( - error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); + error, + reference_interfaces_[ref_itf_dot]->get_value() - + measured_state_values_[itf_dot], + period); } else { @@ -495,31 +556,40 @@ controller_interface::return_type PidController::update_and_write_commands( if (state_publisher_ && state_publisher_->trylock()) { state_publisher_->msg_.header.stamp = time; - for (size_t i = 0; i < dof_; ++i) + for (size_t i = 0; i < dof_names_.size(); ++i) { - state_publisher_->msg_.dof_states[i].reference = reference_interfaces_[i]; - state_publisher_->msg_.dof_states[i].feedback = measured_state_values_[i]; - if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + const auto itf = reference_and_state_dof_names_[i] + itf_; + const auto itf_dot = reference_and_state_dof_names_[i] + itf_dot_; + const auto ref_itf = node_name + itf; + const auto ref_itf_dot = node_name + itf_dot; + state_publisher_->msg_.dof_states[i].reference = + reference_interfaces_[ref_itf]->get_value(); + state_publisher_->msg_.dof_states[i].feedback = measured_state_values_[itf]; + if ( + reference_interfaces_.size() == 2 * dof_names_.size() && + measured_state_values_.size() == 2 * dof_names_.size()) { - state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[dof_ + i]; + state_publisher_->msg_.dof_states[i].feedback_dot = measured_state_values_[itf_dot]; } state_publisher_->msg_.dof_states[i].error = - reference_interfaces_[i] - measured_state_values_[i]; + reference_interfaces_[ref_itf]->get_value() - measured_state_values_[itf]; if (params_.gains.dof_names_map[params_.dof_names[i]].angle_wraparound) { // for continuous angles the error is normalized between -pimsg_.dof_states[i].error = - angles::shortest_angular_distance(measured_state_values_[i], reference_interfaces_[i]); + state_publisher_->msg_.dof_states[i].error = angles::shortest_angular_distance( + measured_state_values_[itf], reference_interfaces_[ref_itf]->get_value()); } - if (reference_interfaces_.size() == 2 * dof_ && measured_state_values_.size() == 2 * dof_) + if ( + reference_interfaces_.size() == 2 * dof_names_.size() && + measured_state_values_.size() == 2 * dof_names_.size()) { state_publisher_->msg_.dof_states[i].error_dot = - reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i]; + reference_interfaces_[ref_itf_dot]->get_value() - measured_state_values_[itf_dot]; } state_publisher_->msg_.dof_states[i].time_step = period.seconds(); // Command can store the old calculated values. This should be obvious because at least one // another value is NaN. - state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); + state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value(); } state_publisher_->unlockAndPublish(); } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 9b53dccb23..e422a4c8fd 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -67,7 +67,7 @@ TEST_F(PidControllerTest, 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(), dof_command_values_.size()); + ASSERT_EQ(cmd_if_conf.names.size(), command_itfs_.size()); for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) { EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); @@ -75,7 +75,7 @@ TEST_F(PidControllerTest, 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(), dof_state_values_.size()); + ASSERT_EQ(state_if_conf.names.size(), state_itfs_.size()); size_t si_index = 0; for (const auto & interface : state_interfaces_) { @@ -89,7 +89,7 @@ TEST_F(PidControllerTest, check_exported_interfaces) // check ref itfs auto ref_if_conf = controller_->export_reference_interfaces(); - ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); + ASSERT_EQ(ref_if_conf.size(), state_itfs_.size()); size_t ri_index = 0; for (const auto & interface : state_interfaces_) { @@ -97,9 +97,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); ++ri_index; } } @@ -125,10 +125,12 @@ TEST_F(PidControllerTest, activate_success) EXPECT_TRUE(std::isnan(cmd)); } - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (const auto & interface : controller_->reference_interfaces_) + EXPECT_EQ(controller_->reference_interfaces_.size(), state_itfs_.size()); + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(std::isnan(interface->get_value())); } } @@ -159,17 +161,17 @@ TEST_F(PidControllerTest, reactivate_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[0]->get_value())); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[0]->get_value())); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0])); + ASSERT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[0]->get_value())); ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0])); - ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); + ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), @@ -215,26 +217,31 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) ASSERT_FALSE(controller_->is_in_chained_mode()); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - for (const auto & interface : controller_->reference_interfaces_) + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(std::isnan(interface->get_value())); } std::shared_ptr msg = std::make_shared(); msg->dof_names = dof_names_; msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + for (size_t i = 0; i < command_itfs_.size(); ++i) { - msg->values[i] = dof_command_values_[i]; + msg->values[i] = command_itfs_[i].get_value(); } msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); controller_->input_ref_.writeFromNonRT(msg); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + for (size_t i = 0; i < command_itfs_.size(); ++i) { EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + EXPECT_EQ( + (*(controller_->input_ref_.readFromRT()))->values[i], command_itfs_[i].get_value()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->operator bool()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->holds_value()); + EXPECT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[i]->get_value())); } ASSERT_EQ( @@ -244,8 +251,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off) EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + EXPECT_EQ(controller_->reference_interfaces_.size(), state_itfs_.size()); + for (size_t i = 0; i < command_itfs_.size(); ++i) { EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); } @@ -264,17 +271,19 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on) ASSERT_FALSE(controller_->is_in_chained_mode()); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[0])); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); - for (const auto & interface : controller_->reference_interfaces_) + for (const auto & interface : controller_->ordered_reference_interfaces_) { - EXPECT_TRUE(std::isnan(interface)); + EXPECT_TRUE(interface->operator bool()); + EXPECT_TRUE(interface->holds_value()); + EXPECT_TRUE(std::isnan(interface->get_value())); } std::shared_ptr msg = std::make_shared(); msg->dof_names = dof_names_; msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + for (size_t i = 0; i < command_itfs_.size(); ++i) { - msg->values[i] = dof_command_values_[i]; + msg->values[i] = command_itfs_[i].get_value(); } msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); controller_->input_ref_.writeFromNonRT(msg); @@ -282,11 +291,14 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on) controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + for (size_t i = 0; i < command_itfs_.size(); ++i) { EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + EXPECT_EQ( + (*(controller_->input_ref_.readFromRT()))->values[i], command_itfs_[i].get_value()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->operator bool()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->holds_value()); + EXPECT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[i]->get_value())); } ASSERT_EQ( @@ -296,8 +308,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on) EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + EXPECT_EQ(controller_->reference_interfaces_.size(), state_itfs_.size()); + for (size_t i = 0; i < command_itfs_.size(); ++i) { EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); } @@ -318,18 +330,21 @@ TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) std::shared_ptr msg = std::make_shared(); msg->dof_names = dof_names_; msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + for (size_t i = 0; i < command_itfs_.size(); ++i) { - msg->values[i] = dof_command_values_[i]; + msg->values[i] = command_itfs_[i].get_value(); } msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); controller_->input_ref_.writeFromNonRT(msg); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + for (size_t i = 0; i < command_itfs_.size(); ++i) { EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + EXPECT_EQ( + (*(controller_->input_ref_.readFromRT()))->values[i], command_itfs_[i].get_value()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->operator bool()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->holds_value()); + EXPECT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[i]->get_value())); } ASSERT_EQ( @@ -340,11 +355,12 @@ TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_off) EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::OFF); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + EXPECT_EQ(controller_->reference_interfaces_.size(), state_itfs_.size()); + for (size_t i = 0; i < command_itfs_.size(); ++i) { EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_EQ( + (*(controller_->input_ref_.readFromRT()))->values[i], command_itfs_[i].get_value()); } } @@ -364,9 +380,9 @@ TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) std::shared_ptr msg = std::make_shared(); msg->dof_names = dof_names_; msg->values.resize(dof_names_.size(), 0.0); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + for (size_t i = 0; i < command_itfs_.size(); ++i) { - msg->values[i] = dof_command_values_[i]; + msg->values[i] = command_itfs_[i].get_value(); } msg->values_dot.resize(dof_names_.size(), std::numeric_limits::quiet_NaN()); controller_->input_ref_.writeFromNonRT(msg); @@ -374,11 +390,14 @@ TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) controller_->control_mode_.writeFromNonRT(feedforward_mode_type::ON); EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + for (size_t i = 0; i < command_itfs_.size(); ++i) { EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + EXPECT_EQ( + (*(controller_->input_ref_.readFromRT()))->values[i], command_itfs_[i].get_value()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->operator bool()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->holds_value()); + EXPECT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[i]->get_value())); } ASSERT_EQ( @@ -389,11 +408,12 @@ TEST_F(PidControllerTest, test_update_logic_chainable_feedforward_on) EXPECT_EQ(*(controller_->control_mode_.readFromRT()), feedforward_mode_type::ON); EXPECT_EQ( controller_->reference_interfaces_.size(), dof_names_.size() * state_interfaces_.size()); - EXPECT_EQ(controller_->reference_interfaces_.size(), dof_state_values_.size()); - for (size_t i = 0; i < dof_command_values_.size(); ++i) + EXPECT_EQ(controller_->reference_interfaces_.size(), state_itfs_.size()); + for (size_t i = 0; i < command_itfs_.size(); ++i) { EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->values[i])); - EXPECT_EQ((*(controller_->input_ref_.readFromRT()))->values[i], dof_command_values_[i]); + EXPECT_EQ( + (*(controller_->input_ref_.readFromRT()))->values[i], command_itfs_[i].get_value()); } } @@ -460,7 +480,7 @@ TEST_F(PidControllerTest, subscribe_and_get_messages_success) { ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); - ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + ASSERT_EQ(msg.dof_states[i].output, command_itfs_[i].get_value()); } } @@ -485,29 +505,33 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) { ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); EXPECT_TRUE(std::isnan(msg.dof_states[i].reference)); - ASSERT_EQ(msg.dof_states[i].output, dof_command_values_[i]); + ASSERT_EQ(msg.dof_states[i].output, command_itfs_[i].get_value()); } - for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + for (size_t i = 0; i < controller_->ordered_reference_interfaces_.size(); ++i) { - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->operator bool()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->holds_value()); + EXPECT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[i]->get_value())); } publish_commands(); controller_->wait_for_commands(executor); - for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + for (size_t i = 0; i < controller_->ordered_reference_interfaces_.size(); ++i) { - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[i])); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->operator bool()); + EXPECT_TRUE(controller_->ordered_reference_interfaces_[i]->holds_value()); + EXPECT_TRUE(std::isnan(controller_->ordered_reference_interfaces_[i]->get_value())); } ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - for (size_t i = 0; i < controller_->reference_interfaces_.size(); ++i) + for (size_t i = 0; i < controller_->ordered_reference_interfaces_.size(); ++i) { - ASSERT_EQ(controller_->reference_interfaces_[i], 0.45); + ASSERT_EQ(controller_->ordered_reference_interfaces_[i]->get_value(), 0.45); } subscribe_and_get_messages(msg); @@ -517,7 +541,7 @@ TEST_F(PidControllerTest, receive_message_and_publish_updated_status) { ASSERT_EQ(msg.dof_states[i].name, dof_names_[i]); ASSERT_EQ(msg.dof_states[i].reference, 0.45); - ASSERT_NE(msg.dof_states[i].output, dof_command_values_[i]); + ASSERT_NE(msg.dof_states[i].output, command_itfs_[i].get_value()); } } diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 158b5d9147..6e2e509964 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -26,6 +26,7 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "pid_controller/pid_controller.hpp" @@ -70,7 +71,7 @@ class TestablePidController : public pid_controller::PidController controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override { - auto ref_itfs = on_export_reference_interfaces(); + auto ref_itfs = export_state_interface_descriptions(); return pid_controller::PidController::on_activate(previous_state); } @@ -111,8 +112,6 @@ class PidControllerFixture : public ::testing::Test dof_names_ = {"joint1"}; command_interface_ = "position"; state_interfaces_ = {"position"}; - dof_state_values_ = {1.1}; - dof_command_values_ = {101.101}; reference_and_state_dof_names_ = {"joint1state"}; // initialize controller @@ -144,8 +143,10 @@ class PidControllerFixture : public ::testing::Test for (size_t i = 0; i < dof_names_.size(); ++i) { - command_itfs_.emplace_back(hardware_interface::CommandInterface( - dof_names_[i], command_interface_, &dof_command_values_[i])); + command_itfs_.emplace_back( + hardware_interface::CommandInterface(hardware_interface::InterfaceDescription( + dof_names_[i], + hardware_interface::InterfaceInfo(command_interface_, "double", "101.101")))); command_ifs.emplace_back(command_itfs_.back()); } @@ -158,7 +159,8 @@ class PidControllerFixture : public ::testing::Test for (const auto & dof_name : dof_names_) { state_itfs_.emplace_back( - hardware_interface::StateInterface(dof_name, interface, &dof_state_values_[index])); + hardware_interface::StateInterface(hardware_interface::InterfaceDescription( + dof_name, hardware_interface::InterfaceInfo(interface, "double", "1.1")))); state_ifs.emplace_back(state_itfs_.back()); ++index; } @@ -264,8 +266,6 @@ class PidControllerFixture : public ::testing::Test std::vector dof_names_; std::string command_interface_; std::vector state_interfaces_; - std::vector dof_state_values_; - std::vector dof_command_values_; std::vector reference_and_state_dof_names_; std::vector state_itfs_; diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 498ca633da..9147beab11 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -54,7 +54,7 @@ TEST_F(PidControllerTest, 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(), dof_command_values_.size()); + ASSERT_EQ(cmd_if_conf.names.size(), command_itfs_.size()); for (size_t i = 0; i < cmd_if_conf.names.size(); ++i) { EXPECT_EQ(cmd_if_conf.names[i], dof_names_[i] + "/" + command_interface_); @@ -62,7 +62,7 @@ TEST_F(PidControllerTest, 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(), dof_state_values_.size()); + ASSERT_EQ(state_if_conf.names.size(), state_itfs_.size()); size_t si_index = 0; for (const auto & interface : state_interfaces_) { @@ -76,7 +76,7 @@ TEST_F(PidControllerTest, check_exported_interfaces) // check ref itfs auto ref_if_conf = controller_->export_reference_interfaces(); - ASSERT_EQ(ref_if_conf.size(), dof_state_values_.size()); + ASSERT_EQ(ref_if_conf.size(), state_itfs_.size()); size_t ri_index = 0; for (const auto & interface : state_interfaces_) { @@ -84,9 +84,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/position_controllers/test/test_joint_group_position_controller.cpp b/position_controllers/test/test_joint_group_position_controller.cpp index 96a5cead17..320292b4c3 100644 --- a/position_controllers/test/test_joint_group_position_controller.cpp +++ b/position_controllers/test/test_joint_group_position_controller.cpp @@ -105,9 +105,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -120,9 +120,9 @@ TEST_F(JointGroupPositionControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); } TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) @@ -142,9 +142,9 @@ TEST_F(JointGroupPositionControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); } TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) @@ -159,9 +159,9 @@ TEST_F(JointGroupPositionControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); } TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) @@ -170,9 +170,9 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -203,7 +203,7 @@ TEST_F(JointGroupPositionControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); } diff --git a/position_controllers/test/test_joint_group_position_controller.hpp b/position_controllers/test/test_joint_group_position_controller.hpp index fefd2073e3..bd26c6e0d5 100644 --- a/position_controllers/test/test_joint_group_position_controller.hpp +++ b/position_controllers/test/test_joint_group_position_controller.hpp @@ -22,12 +22,15 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "position_controllers/joint_group_position_controller.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_POSITION; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // subclassing and friending so we can access member variables class FriendJointGroupPositionController : public position_controllers::JointGroupPositionController @@ -56,9 +59,12 @@ class JointGroupPositionControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]}; - CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]}; - CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]}; + CommandInterface joint_1_pos_cmd_{ + InterfaceDescription(joint_names_[0], InterfaceInfo(HW_IF_POSITION, "1.1", "double"))}; + CommandInterface joint_2_pos_cmd_{ + InterfaceDescription(joint_names_[1], InterfaceInfo(HW_IF_POSITION, "2.1", "double"))}; + CommandInterface joint_3_pos_cmd_{ + InterfaceDescription(joint_names_[2], InterfaceInfo(HW_IF_POSITION, "3.1", "double"))}; rclcpp::executors::SingleThreadedExecutor executor; }; diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp index 59d27ebc0c..910f8fe487 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.cpp @@ -208,7 +208,7 @@ TEST_F(RangeSensorBroadcasterTest, Publish_RangeBroadcaster_Success) subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(range_.get_value())); EXPECT_EQ(range_msg.radiation_type, radiation_type_); EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); @@ -227,11 +227,11 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) sensor_msgs::msg::Range range_msg; - sensor_range_ = 0.10; + range_.set_value(0.10); subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(range_.get_value())); EXPECT_EQ(range_msg.radiation_type, radiation_type_); EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); @@ -240,11 +240,11 @@ TEST_F(RangeSensorBroadcasterTest, Publish_Bandaries_RangeBroadcaster_Success) EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); #endif - sensor_range_ = 4.0; + range_.set_value(4.0); subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(range_.get_value())); EXPECT_EQ(range_msg.radiation_type, radiation_type_); EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); @@ -263,12 +263,12 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe sensor_msgs::msg::Range range_msg; - sensor_range_ = 0.0; + range_.set_value(0.0); subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); // Even out of boundaries you will get the out_of_range range value - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(range_.get_value())); EXPECT_EQ(range_msg.radiation_type, radiation_type_); EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); @@ -277,12 +277,12 @@ TEST_F(RangeSensorBroadcasterTest, Publish_OutOfBandaries_RangeBroadcaster_Succe EXPECT_THAT(range_msg.variance, ::testing::FloatEq(variance_)); #endif - sensor_range_ = 6.0; + range_.set_value(6.0); subscribe_and_get_message(range_msg); EXPECT_EQ(range_msg.header.frame_id, frame_id_); // Even out of boundaries you will get the out_of_range range value - EXPECT_THAT(range_msg.range, ::testing::FloatEq(sensor_range_)); + EXPECT_THAT(range_msg.range, ::testing::FloatEq(range_.get_value())); EXPECT_EQ(range_msg.radiation_type, radiation_type_); EXPECT_THAT(range_msg.field_of_view, ::testing::FloatEq(field_of_view_)); EXPECT_THAT(range_msg.min_range, ::testing::FloatEq(min_range_)); diff --git a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp index 10696d071f..0a109c00c8 100644 --- a/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp +++ b/range_sensor_broadcaster/test/test_range_sensor_broadcaster.hpp @@ -25,6 +25,7 @@ #include "gmock/gmock.h" +#include "hardware_interface/hardware_info.hpp" #include "range_sensor_broadcaster/range_sensor_broadcaster.hpp" class RangeSensorBroadcasterTest : public ::testing::Test @@ -45,8 +46,8 @@ class RangeSensorBroadcasterTest : public ::testing::Test const double max_range_ = 7.0; const double variance_ = 1.0; - double sensor_range_ = 3.1; - hardware_interface::StateInterface range_{sensor_name_, "range", &sensor_range_}; + hardware_interface::StateInterface range_{hardware_interface::InterfaceDescription( + sensor_name_, hardware_interface::InterfaceInfo("range", "double", "3.1"))}; std::unique_ptr range_broadcaster_; 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 84a892d79e..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 @@ -105,7 +105,10 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl std::unique_ptr rt_tf_odom_state_publisher_; // override methods from ChainableControllerInterface - std::vector on_export_reference_interfaces() override; + std::vector export_state_interface_descriptions() + override; + std::vector export_reference_interface_descriptions() + override; bool on_set_chained_mode(bool chained_mode) override; @@ -132,6 +135,9 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl std::vector rear_wheels_state_names_; std::vector front_wheels_state_names_; + std::string lin_ref_itf_; + std::string ang_ref_itf_; + private: // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f193098f82..cb4bc9ef95 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -20,6 +20,8 @@ #include #include +#include "controller_interface/helpers.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -28,6 +30,8 @@ namespace using ControllerTwistReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // called from RT control loop void reset_controller_reference_msg( @@ -307,21 +311,29 @@ SteeringControllersLibrary::state_interface_configuration() const return state_interfaces_config; } -std::vector -SteeringControllersLibrary::on_export_reference_interfaces() +std::vector +SteeringControllersLibrary::export_state_interface_descriptions() { - reference_interfaces_.resize(nr_ref_itfs_, std::numeric_limits::quiet_NaN()); + return {}; +} - std::vector reference_interfaces; +std::vector +SteeringControllersLibrary::export_reference_interface_descriptions() +{ + using hardware_interface::InterfaceDescription; + using hardware_interface::InterfaceInfo; + std::vector reference_interfaces; reference_interfaces.reserve(nr_ref_itfs_); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[0])); + reference_interfaces.emplace_back(InterfaceDescription( + get_node()->get_name(), + InterfaceInfo(std::string("linear/") + hardware_interface::HW_IF_VELOCITY, "double"))); + lin_ref_itf_ = reference_interfaces.back().get_name(); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY, - &reference_interfaces_[1])); + reference_interfaces.emplace_back(InterfaceDescription( + get_node()->get_name(), + InterfaceInfo(std::string("angular/") + hardware_interface::HW_IF_POSITION, "double"))); + ang_ref_itf_ = reference_interfaces.back().get_name(); return reference_interfaces; } @@ -362,16 +374,16 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f { if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; + reference_interfaces_[lin_ref_itf_]->set_value(current_ref->twist.linear.x); + reference_interfaces_[ang_ref_itf_]->set_value(current_ref->twist.angular.z); } } else { if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; + reference_interfaces_[lin_ref_itf_]->set_value(0.0); + reference_interfaces_[ang_ref_itf_]->set_value(0.0); current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); } @@ -390,11 +402,13 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c // Limit velocities and accelerations: // TODO(destogl): add limiter for the velocities - if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1])) + if ( + !std::isnan(reference_interfaces_[lin_ref_itf_]->get_value()) && + !std::isnan(reference_interfaces_[ang_ref_itf_]->get_value())) { // store (for open loop odometry) and set commands - last_linear_velocity_ = reference_interfaces_[0]; - last_angular_velocity_ = reference_interfaces_[1]; + last_linear_velocity_ = reference_interfaces_[lin_ref_itf_]->get_value(); + last_angular_velocity_ = reference_interfaces_[ang_ref_itf_]->get_value(); auto [traction_commands, steering_commands] = odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); @@ -478,30 +492,30 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c if (params_.position_feedback) { controller_state_publisher_->msg_.traction_wheels_position.push_back( - state_interfaces_[i].get_value()); + state_interfaces_[i].get_value()); } else { controller_state_publisher_->msg_.traction_wheels_velocity.push_back( - state_interfaces_[i].get_value()); + state_interfaces_[i].get_value()); } controller_state_publisher_->msg_.linear_velocity_command.push_back( - command_interfaces_[i].get_value()); + command_interfaces_[i].get_value()); } for (size_t i = 0; i < number_of_steering_wheels; ++i) { controller_state_publisher_->msg_.steer_positions.push_back( - state_interfaces_[number_of_traction_wheels + i].get_value()); + state_interfaces_[number_of_traction_wheels + i].get_value()); controller_state_publisher_->msg_.steering_angle_command.push_back( - command_interfaces_[number_of_traction_wheels + i].get_value()); + command_interfaces_[number_of_traction_wheels + i].get_value()); } controller_state_publisher_->unlockAndPublish(); } - reference_interfaces_[0] = std::numeric_limits::quiet_NaN(); - reference_interfaces_[1] = std::numeric_limits::quiet_NaN(); + reference_interfaces_[lin_ref_itf_]->set_value(std::numeric_limits::quiet_NaN()); + reference_interfaces_[ang_ref_itf_]->set_value(std::numeric_limits::quiet_NaN()); return controller_interface::return_type::OK; } diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index fca7d00946..c6ab40fd65 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -32,7 +32,7 @@ TEST_F(SteeringControllersLibraryTest, 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_size_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); @@ -48,7 +48,7 @@ TEST_F(SteeringControllersLibraryTest, 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_size_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); @@ -70,9 +70,9 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } @@ -91,9 +91,11 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_FALSE(controller_->is_in_chained_mode()); - for (const auto & interface : controller_->reference_interfaces_) + 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())); } // set command statically @@ -127,24 +129,25 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); - for (const auto & interface : controller_->reference_interfaces_) + 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())); } EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - for (const auto & interface : controller_->reference_interfaces_) + 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())); } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0); } // case 2 position_feedback = true @@ -168,24 +171,25 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[1])); - for (const auto & interface : controller_->reference_interfaces_) + 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())); } EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); - EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); - for (const auto & interface : controller_->reference_interfaces_) + 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())); } for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); } } diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 93ee823e0f..09a81312d9 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -15,14 +15,14 @@ #ifndef TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ #define TEST_STEERING_CONTROLLERS_LIBRARY_HPP_ -#include - #include #include #include #include #include +#include "gmock/gmock.h" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "rclcpp/executor.hpp" @@ -35,6 +35,8 @@ using ControllerStateMsg = steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; using ControllerReferenceMsg = steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // NOTE: Testing steering_controllers_library for ackermann vehicle configuration only @@ -84,7 +86,7 @@ class TestableSteeringControllersLibrary controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override { - auto ref_itfs = on_export_reference_interfaces(); + auto ref_itf_descriptions = export_reference_interface_descriptions(); return steering_controllers_library::SteeringControllersLibrary::on_activate(previous_state); } @@ -168,51 +170,43 @@ class SteeringControllersLibraryFixture : public ::testing::Test } std::vector command_ifs; - command_itfs_.reserve(joint_command_values_.size()); - command_ifs.reserve(joint_command_values_.size()); + command_itfs_.reserve(joint_command_size_); + command_ifs.reserve(joint_command_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(InterfaceDescription( + rear_wheels_names_[0], InterfaceInfo(traction_interface_name_, "1.1", "double")))); 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(InterfaceDescription( + rear_wheels_names_[1], InterfaceInfo(steering_interface_name_, "3.3", "double")))); 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(InterfaceDescription( + front_wheels_names_[0], InterfaceInfo(steering_interface_name_, "2.2", "double")))); 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(InterfaceDescription( + front_wheels_names_[1], InterfaceInfo(steering_interface_name_, "4.4", "double")))); 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_.reserve(joint_state_size_); + state_ifs.reserve(joint_state_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(InterfaceDescription( + rear_wheels_names_[0], InterfaceInfo(traction_interface_name_, "0.5", "double")))); 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(InterfaceDescription( + rear_wheels_names_[1], InterfaceInfo(traction_interface_name_, "0.5", "double")))); 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(InterfaceDescription( + front_wheels_names_[0], InterfaceInfo(steering_interface_name_, "0.0", "double")))); 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(InterfaceDescription( + front_wheels_names_[1], InterfaceInfo(steering_interface_name_, "0.0", "double")))); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -314,8 +308,8 @@ class SteeringControllersLibraryFixture : 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_state_size_ = 4; + const size_t joint_command_size_ = 4; std::array joint_reference_interfaces_ = {"linear/velocity", "angular/velocity"}; std::string steering_interface_name_ = "position"; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 94ff63e659..15b6b93c31 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -116,8 +116,8 @@ controller_interface::return_type TricycleController::update( TwistStamped command = *last_command_msg; double & linear_command = command.twist.linear.x; double & angular_command = command.twist.angular.z; - double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s - double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians + double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s + double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians if (params_.open_loop) { diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp index 9d43c2590d..79842af2d8 100644 --- a/tricycle_controller/test/test_tricycle_controller.cpp +++ b/tricycle_controller/test/test_tricycle_controller.cpp @@ -24,6 +24,7 @@ #include #include +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -35,6 +36,8 @@ using CallbackReturn = controller_interface::CallbackReturn; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; using hardware_interface::LoanedCommandInterface; using hardware_interface::LoanedStateInterface; using lifecycle_msgs::msg::State; @@ -175,17 +178,17 @@ class TestTricycleController : public ::testing::Test double position_ = 0.1; double velocity_ = 0.2; - hardware_interface::StateInterface steering_joint_pos_state_{ - steering_joint_name, HW_IF_POSITION, &position_}; + hardware_interface::StateInterface steering_joint_pos_state_{InterfaceDescription( + steering_joint_name, InterfaceInfo(HW_IF_POSITION, std::to_string(position_), "double"))}; - hardware_interface::StateInterface traction_joint_vel_state_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; + hardware_interface::StateInterface traction_joint_vel_state_{InterfaceDescription( + traction_joint_name, InterfaceInfo(HW_IF_VELOCITY, std::to_string(velocity_), "double"))}; - hardware_interface::CommandInterface steering_joint_pos_cmd_{ - steering_joint_name, HW_IF_POSITION, &position_}; + hardware_interface::CommandInterface steering_joint_pos_cmd_{InterfaceDescription( + steering_joint_name, InterfaceInfo(HW_IF_POSITION, std::to_string(position_), "double"))}; - hardware_interface::CommandInterface traction_joint_vel_cmd_{ - traction_joint_name, HW_IF_VELOCITY, &velocity_}; + hardware_interface::CommandInterface traction_joint_vel_cmd_{InterfaceDescription( + traction_joint_name, InterfaceInfo(HW_IF_VELOCITY, std::to_string(velocity_), "double"))}; rclcpp::Node::SharedPtr pub_node; rclcpp::Publisher::SharedPtr velocity_publisher; @@ -289,8 +292,8 @@ TEST_F(TestTricycleController, cleanup) ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); // should be stopped - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); executor.cancel(); } @@ -310,8 +313,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) assignResources(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); - EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value()); state = controller_->get_node()->activate(); ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id()); @@ -326,8 +329,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) ASSERT_EQ( controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value()); // deactivated // wait so controller process the second point when deactivated @@ -338,14 +341,16 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters) controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()"; - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) + << "Wheels are halted on deactivate()"; + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) + << "Wheels are halted on deactivate()"; // cleanup state = controller_->get_node()->cleanup(); ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id()); - EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); - EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); + EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()); + EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()); state = controller_->get_node()->configure(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 03be6b88f0..7d354ab393 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -61,10 +61,10 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period else { const double traction_right_wheel_value = - state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); + state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(); const double traction_left_wheel_value = - state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); - const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); + state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(); + const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value(); if ( std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) && std::isfinite(steering_position)) diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 3f2589cb6c..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_); @@ -75,7 +75,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) { @@ -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 3b7a053937..5a1685f9e2 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -79,7 +79,7 @@ class TestableTricycleSteeringController 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 tricycle_steering_controller::TricycleSteeringController::on_activate(previous_state); } @@ -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) { diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp index d781b6ca5c..2b790259c1 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp @@ -105,9 +105,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // send command auto command_ptr = std::make_shared(); @@ -120,9 +120,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest) controller_interface::return_type::OK); // check joint commands have been modified - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) @@ -142,9 +142,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest) controller_interface::return_type::ERROR); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) @@ -159,9 +159,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest) controller_interface::return_type::OK); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); } TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) @@ -170,9 +170,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_->get_node()->set_parameter({"joints", joint_names_}); // default values - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); auto node_state = controller_->get_node()->configure(); ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -203,9 +203,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest) controller_interface::return_type::OK); // check command in handle was set - ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); + ASSERT_EQ(joint_1_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_cmd_.get_value(), 30.0); } TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) @@ -217,15 +217,15 @@ TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are still the default ones - ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); - ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); - ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); + ASSERT_EQ(joint_1_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_cmd_.get_value(), 3.1); // stop the controller ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS); // check joint commands are now zero - ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); - ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_1_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_2_cmd_.get_value(), 0.0); + ASSERT_EQ(joint_3_cmd_.get_value(), 0.0); } diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.hpp b/velocity_controllers/test/test_joint_group_velocity_controller.hpp index 0ebe68833c..609aef6340 100644 --- a/velocity_controllers/test/test_joint_group_velocity_controller.hpp +++ b/velocity_controllers/test/test_joint_group_velocity_controller.hpp @@ -22,13 +22,15 @@ #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" #include "velocity_controllers/joint_group_velocity_controller.hpp" using hardware_interface::CommandInterface; using hardware_interface::HW_IF_VELOCITY; - +using hardware_interface::InterfaceDescription; +using hardware_interface::InterfaceInfo; // subclassing and friending so we can access member variables class FriendJointGroupVelocityController : public velocity_controllers::JointGroupVelocityController { @@ -56,9 +58,12 @@ class JointGroupVelocityControllerTest : public ::testing::Test const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; std::vector joint_commands_ = {1.1, 2.1, 3.1}; - CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]}; - CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]}; - CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]}; + CommandInterface joint_1_cmd_{ + InterfaceDescription(joint_names_[0], InterfaceInfo(HW_IF_VELOCITY, "1.1", "double"))}; + CommandInterface joint_2_cmd_{ + InterfaceDescription(joint_names_[1], InterfaceInfo(HW_IF_VELOCITY, "2.1", "double"))}; + CommandInterface joint_3_cmd_{ + InterfaceDescription(joint_names_[2], InterfaceInfo(HW_IF_VELOCITY, "3.1", "double"))}; rclcpp::executors::SingleThreadedExecutor executor; };