diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index e9b38de65d..c244ee1254 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -32,10 +32,6 @@ namespace mock_components { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -static constexpr size_t POSITION_INTERFACE_INDEX = 0; -static constexpr size_t VELOCITY_INTERFACE_INDEX = 1; -static constexpr size_t ACCELERATION_INTERFACE_INDEX = 2; - class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface { public: @@ -45,14 +41,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector export_command_interfaces() override; - return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) override; - - return_type perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) override; - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override @@ -72,6 +60,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; + const size_t POSITION_INTERFACE_INDEX = 0; + struct MimicJoint { std::size_t joint_index; @@ -125,9 +115,6 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::string custom_interface_with_following_offset_; size_t index_custom_interface_with_following_offset_; - bool calculate_dynamics_; - std::vector joint_control_mode_; - bool command_propagation_disabled_; }; diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 9d2351db36..45ec7c9b7d 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -105,17 +105,6 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i command_propagation_disabled_ = false; } - // check if there is parameter that enables dynamic calculation - it = info_.hardware_parameters.find("calculate_dynamics"); - if (it != info.hardware_parameters.end()) - { - calculate_dynamics_ = hardware_interface::parse_bool(it->second); - } - else - { - calculate_dynamics_ = false; - } - // process parameters about state following position_state_following_offset_ = 0.0; custom_interface_with_following_offset_ = ""; @@ -297,7 +286,7 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (size_t i = 0; i < info_.joints.size(); ++i) + for (auto i = 0u; i < info_.joints.size(); i++) { const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) @@ -318,8 +307,6 @@ std::vector GenericSystem::export_command_ } } } - // Set position control mode per default - joint_control_mode_.resize(info_.joints.size(), POSITION_INTERFACE_INDEX); // Mock sensor command interfaces if (use_mock_sensor_command_interfaces_) @@ -356,135 +343,7 @@ std::vector GenericSystem::export_command_ return command_interfaces; } -return_type GenericSystem::prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & /*stop_interfaces*/) -{ - hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - - const size_t FOUND_ONCE_FLAG = 1000000; - - std::vector joint_found_in_x_requests_; - joint_found_in_x_requests_.resize(info_.joints.size(), 0); - - for (const auto & key : start_interfaces) - { - // check if interface is joint - auto joint_it_found = std::find_if( - info_.joints.begin(), info_.joints.end(), - [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); - - if (joint_it_found != info_.joints.end()) - { - const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); - if (joint_found_in_x_requests_[joint_index] == 0) - { - joint_found_in_x_requests_[joint_index] = FOUND_ONCE_FLAG; - } - - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) - { - joint_found_in_x_requests_[joint_index] += 1; - } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) - { - if (!calculate_dynamics_) - { - RCUTILS_LOG_WARN_NAMED( - "mock_generic_system", - "Requested velocity mode for joint '%s' without dynamics calculation enabled - this " - "might lead to wrong feedback and unexpected behavior.", - info_.joints[joint_index].name.c_str()); - } - joint_found_in_x_requests_[joint_index] += 1; - } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) - { - if (!calculate_dynamics_) - { - RCUTILS_LOG_WARN_NAMED( - "mock_generic_system", - "Requested acceleration mode for joint '%s' without dynamics calculation enabled - " - "this might lead to wrong feedback and unexpected behavior.", - info_.joints[joint_index].name.c_str()); - } - joint_found_in_x_requests_[joint_index] += 1; - } - } - else - { - RCUTILS_LOG_DEBUG_NAMED( - "mock_generic_system", "Got interface '%s' that is not joint - nothing to do!", - key.c_str()); - } - } - - for (size_t i = 0; i < info_.joints.size(); ++i) - { - // There has to always be at least one control mode from the above three set - if (joint_found_in_x_requests_[i] == FOUND_ONCE_FLAG) - { - RCUTILS_LOG_ERROR_NAMED( - "mock_generic_system", "Joint '%s' has to have '%s', '%s', or '%s' interface!", - info_.joints[i].name.c_str(), hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION); - ret_val = hardware_interface::return_type::ERROR; - } - - // Currently we don't support multiple interface request - if (joint_found_in_x_requests_[i] > (FOUND_ONCE_FLAG + 1)) - { - RCUTILS_LOG_ERROR_NAMED( - "mock_generic_system", - "Got multiple (%zu) starting interfaces for joint '%s' - this is not " - "supported!", - joint_found_in_x_requests_[i] - FOUND_ONCE_FLAG, info_.joints[i].name.c_str()); - ret_val = hardware_interface::return_type::ERROR; - } - } - - return ret_val; -} - -return_type GenericSystem::perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & /*stop_interfaces*/) -{ - if (!calculate_dynamics_) - { - return hardware_interface::return_type::OK; - } - - for (const auto & key : start_interfaces) - { - // check if interface is joint - auto joint_it_found = std::find_if( - info_.joints.begin(), info_.joints.end(), - [key](const auto & joint) { return (key.find(joint.name) != std::string::npos); }); - - if (joint_it_found != info_.joints.end()) - { - const size_t joint_index = std::distance(info_.joints.begin(), joint_it_found); - - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_POSITION) - { - joint_control_mode_[joint_index] = POSITION_INTERFACE_INDEX; - } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_VELOCITY) - { - joint_control_mode_[joint_index] = VELOCITY_INTERFACE_INDEX; - } - if (key == info_.joints[joint_index].name + "/" + hardware_interface::HW_IF_ACCELERATION) - { - joint_control_mode_[joint_index] = ACCELERATION_INTERFACE_INDEX; - } - } - } - - return hardware_interface::return_type::OK; -} - -return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { if (command_propagation_disabled_) { @@ -507,97 +366,19 @@ return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Dur } }; + // apply offset to positions only for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) { - if (calculate_dynamics_) + if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) { - switch (joint_control_mode_[j]) - { - case ACCELERATION_INTERFACE_INDEX: - { - // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - - joint_states_[VELOCITY_INTERFACE_INDEX][j] += - joint_states_[ACCELERATION_INTERFACE_INDEX][j] * period.seconds(); - - if (!std::isnan(joint_commands_[ACCELERATION_INTERFACE_INDEX][j])) - { - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - joint_commands_[ACCELERATION_INTERFACE_INDEX][j]; - } - break; - } - case VELOCITY_INTERFACE_INDEX: - { - // currently we do backward integration - joint_states_[POSITION_INTERFACE_INDEX][j] += // apply offset to positions only - joint_states_[VELOCITY_INTERFACE_INDEX][j] * period.seconds() + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - - if (!std::isnan(joint_commands_[VELOCITY_INTERFACE_INDEX][j])) - { - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - joint_commands_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); - } - break; - } - case POSITION_INTERFACE_INDEX: - { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) - { - const double old_position = joint_states_[POSITION_INTERFACE_INDEX][j]; - const double old_velocity = joint_states_[VELOCITY_INTERFACE_INDEX][j]; - - joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - - joint_states_[VELOCITY_INTERFACE_INDEX][j] = - (joint_states_[POSITION_INTERFACE_INDEX][j] - old_position) / period.seconds(); - - joint_states_[ACCELERATION_INTERFACE_INDEX][j] = - (joint_states_[VELOCITY_INTERFACE_INDEX][j] - old_velocity) / period.seconds(); - } - break; - } - } - } - else - { - for (size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size(); ++j) - { - if (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) - { - joint_states_[POSITION_INTERFACE_INDEX][j] = // apply offset to positions only - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ - : 0.0); - } - } + joint_states_[POSITION_INTERFACE_INDEX][j] = + joint_commands_[POSITION_INTERFACE_INDEX][j] + + (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); } } - // do loopback on all other interfaces - starts from 1 or 3 because 0, 1, 3 are position, - // velocity, and acceleration interface - if (calculate_dynamics_) - { - mirror_command_to_state(joint_states_, joint_commands_, 3); - } - else - { - mirror_command_to_state(joint_states_, joint_commands_, 1); - } + // do loopback on all other interfaces - starts from 1 because 0 index is position interface + mirror_command_to_state(joint_states_, joint_commands_, 1); for (const auto & mimic_joint : mimic_joints_) { diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 1aec4a6074..71fe09a0ff 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -33,8 +33,7 @@ namespace { const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math -const auto COMPARE_DELTA = 0.0001; +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); } // namespace class TestGenericSystem : public ::testing::Test @@ -493,39 +492,6 @@ class TestGenericSystem : public ::testing::Test )"; - hardware_system_2dof_standard_interfaces_with_different_control_modes_ = - R"( - - - mock_components/GenericSystem - true - - - - - - 3.45 - - - - - - - - - 2.78 - - - - - - - - - - -)"; - disabled_commands_ = R"( @@ -561,7 +527,6 @@ class TestGenericSystem : public ::testing::Test std::string valid_urdf_ros2_control_system_robot_with_gpio_mock_command_True_; std::string sensor_with_initial_value_; std::string gpio_with_initial_value_; - std::string hardware_system_2dof_standard_interfaces_with_different_control_modes_; std::string disabled_commands_; }; @@ -1637,202 +1602,6 @@ TEST_F(TestGenericSystem, gpio_with_initial_value) ASSERT_EQ(1, state.get_value()); } -TEST_F(TestGenericSystem, simple_dynamics_pos_vel_acc_control_modes_interfaces) -{ - auto urdf = ros2_control_test_assets::urdf_head + - hardware_system_2dof_standard_interfaces_with_different_control_modes_ + - ros2_control_test_assets::urdf_tail; - - TestableResourceManager rm(urdf); - // Activate components to get all interfaces available - activate_components(rm, {"HardwareSystem2dofStandardInterfacesWithDifferentControlModes"}); - - // Check interfaces - EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_EQ(7u, rm.state_interface_keys().size()); - EXPECT_TRUE(rm.state_interface_exists("joint1/position")); - EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.state_interface_exists("joint1/acceleration")); - EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); - EXPECT_TRUE(rm.state_interface_exists("joint2/velocity")); - EXPECT_TRUE(rm.state_interface_exists("joint2/acceleration")); - EXPECT_TRUE(rm.state_interface_exists("flange_vacuum/vacuum")); - - ASSERT_EQ(5u, rm.command_interface_keys().size()); - EXPECT_TRUE(rm.command_interface_exists("joint1/position")); - EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); - EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); - EXPECT_TRUE(rm.command_interface_exists("joint2/acceleration")); - EXPECT_TRUE(rm.command_interface_exists("flange_vacuum/vacuum")); - - // Check initial values - hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); - hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity"); - hardware_interface::LoanedStateInterface j1a_s = rm.claim_state_interface("joint1/acceleration"); - hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface("joint2/position"); - hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface("joint2/velocity"); - hardware_interface::LoanedStateInterface j2a_s = rm.claim_state_interface("joint2/acceleration"); - hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface("joint1/position"); - hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface("joint1/velocity"); - hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface("joint2/velocity"); - hardware_interface::LoanedCommandInterface j2a_c = - rm.claim_command_interface("joint2/acceleration"); - - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_TRUE(std::isnan(j1p_c.get_value())); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_TRUE(std::isnan(j2a_c.get_value())); - - // Test error management in prepare mode switch - ASSERT_EQ( // joint2 has non 'position', 'velocity', or 'acceleration' interface - rm.prepare_command_mode_switch({"joint1/position", "joint2/effort"}, {}), false); - ASSERT_EQ( // joint1 has two interfaces - rm.prepare_command_mode_switch({"joint1/position", "joint1/acceleration"}, {}), false); - - // switch controller mode as controller manager is doing - gpio itf 'vacuum' will be ignored - ASSERT_EQ( - rm.prepare_command_mode_switch( - {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), - true); - ASSERT_EQ( - rm.perform_command_mode_switch( - {"joint1/position", "joint2/acceleration", "flange_vacuum/vacuum"}, {}), - true); - - // set some new values in commands - j1p_c.set_value(0.11); - j2a_c.set_value(3.5); - - // State values should not be changed - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); - - // write() does not change values - rm.write(TIME, PERIOD); - EXPECT_EQ(3.45, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); - - // read() mirrors commands to states and calculate dynamics - rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(-33.4, j1v_s.get_value()); - EXPECT_NEAR(-334.0, j1a_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_EQ(0.0, j2v_s.get_value()); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); - - // read() mirrors commands to states and calculate dynamics - rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_NEAR(334.0, j1a_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(2.78, j2p_s.get_value()); - EXPECT_NEAR(0.35, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); - - // read() mirrors commands to states and calculate dynamics - rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_TRUE(std::isnan(j1v_c.get_value())); - ASSERT_TRUE(std::isnan(j2v_c.get_value())); - ASSERT_EQ(3.5, j2a_c.get_value()); - - // switch controller mode as controller manager is doing - ASSERT_EQ(rm.prepare_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); - ASSERT_EQ(rm.perform_command_mode_switch({"joint1/velocity", "joint2/velocity"}, {}), true); - - // set some new values in commands - j1v_c.set_value(0.5); - j2v_c.set_value(2.0); - - // State values should not be changed - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); - - // write() does not change values - rm.write(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.0, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(2.815, j2p_s.get_value()); - EXPECT_NEAR(0.7, j2v_s.get_value(), COMPARE_DELTA); - EXPECT_EQ(3.5, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); - - // read() mirrors commands to states and calculate dynamics (both velocity mode) - rm.read(TIME, PERIOD); - EXPECT_EQ(0.11, j1p_s.get_value()); - EXPECT_EQ(0.5, j1v_s.get_value()); - EXPECT_EQ(5.0, j1a_s.get_value()); - EXPECT_EQ(2.885, j2p_s.get_value()); - EXPECT_EQ(2.0, j2v_s.get_value()); - EXPECT_NEAR(13.0, j2a_s.get_value(), COMPARE_DELTA); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); - - // read() mirrors commands to states and calculate dynamics (both velocity mode) - rm.read(TIME, PERIOD); - EXPECT_EQ(0.16, j1p_s.get_value()); - EXPECT_EQ(0.5, j1v_s.get_value()); - EXPECT_EQ(0.0, j1a_s.get_value()); - EXPECT_EQ(3.085, j2p_s.get_value()); - EXPECT_EQ(2.0, j2v_s.get_value()); - EXPECT_EQ(0.0, j2a_s.get_value()); - ASSERT_EQ(0.11, j1p_c.get_value()); - ASSERT_EQ(0.5, j1v_c.get_value()); - ASSERT_EQ(2.0, j2v_c.get_value()); - ASSERT_EQ(3.5, j2a_c.get_value()); -} - TEST_F(TestGenericSystem, disabled_commands_flag_is_active) { auto urdf = @@ -1850,6 +1619,7 @@ TEST_F(TestGenericSystem, disabled_commands_flag_is_active) ASSERT_EQ(2u, rm.command_interface_keys().size()); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); EXPECT_TRUE(rm.command_interface_exists("joint1/velocity")); + // Check initial values hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface("joint1/position"); hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface("joint1/velocity");