diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index d1ea84222c..2e1400f718 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - - uses: actions/setup-python@v4.7.0 + - uses: actions/setup-python@v4.7.1 with: python-version: '3.10' - name: Install system hooks diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst index aaa2667a06..76b09e75a3 100644 --- a/controller_interface/CHANGELOG.rst +++ b/controller_interface/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package controller_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- +* Enable services for setting the log-level in controller per default (`#1102 `_) +* Contributors: Dr. Denis + 3.18.0 (2023-08-17) ------------------- * add a broadcaster for range sensor (`#1091 `_) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index d960bdf28a..b9c89c0bfb 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -2,7 +2,7 @@ controller_interface - 3.18.0 + 3.19.1 Description of controller_interface Bence Magyar Denis Štogl diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst index 2a9e348b8e..0da7427315 100644 --- a/controller_manager/CHANGELOG.rst +++ b/controller_manager/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- +* Fix next controller update cycle time clock (`#1127 `_) +* Contributors: Sai Kishor Kothakota + +3.19.0 (2023-10-03) +------------------- +* Proper controller update rate (`#1105 `_) +* Fix multiple calls to export reference interfaces (`#1108 `_) +* [Docs] Fix information about activation and deactivation of chainable controllers (`#1104 `_) +* Contributors: Dr. Denis, Sai Kishor Kothakota + 3.18.0 (2023-08-17) ------------------- * Controller sorting and proper execution in a chain (Fixes `#853 `_) (`#1063 `_) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 685c489e23..79a9861463 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -58,7 +58,7 @@ namespace controller_manager { using ControllersListIterator = std::vector::const_iterator; -rclcpp::NodeOptions get_cm_node_options(); +CONTROLLER_MANAGER_PUBLIC rclcpp::NodeOptions get_cm_node_options(); class ControllerManager : public rclcpp::Node { @@ -122,6 +122,7 @@ class ControllerManager : public rclcpp::Node controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + controller_spec.next_update_cycle_time = std::make_shared(0); return add_controller_impl(controller_spec); } diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 63823f8d33..6f7483f3ec 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -20,6 +20,7 @@ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #include +#include #include #include #include "controller_interface/controller_interface.hpp" @@ -37,6 +38,7 @@ struct ControllerSpec { hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; + std::shared_ptr next_update_cycle_time; }; } // namespace controller_manager diff --git a/controller_manager/package.xml b/controller_manager/package.xml index adb6769d9c..556c1f222e 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -2,7 +2,7 @@ controller_manager - 3.18.0 + 3.19.1 Description of controller_manager Bence Magyar Denis Štogl diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f9799a74af..1a526a47f8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -566,6 +566,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; + controller_spec.next_update_cycle_time = std::make_shared( + 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); return add_controller_impl(controller_spec); } @@ -741,16 +743,13 @@ controller_interface::return_type ControllerManager::configure_controller( } else if (controller_update_rate != 0 && cm_update_rate % controller_update_rate != 0) { - // NOTE: The following computation is done to compute the approx controller update that can be - // achieved w.r.t to the CM's update rate. This is done this way to take into account the - // unsigned integer division. - const auto act_ctrl_update_rate = cm_update_rate / (cm_update_rate / controller_update_rate); RCLCPP_WARN( get_logger(), - "The controller : %s update rate : %d Hz is not a perfect divisor of the controller " - "manager's update rate : %d Hz!. The controller will be updated with nearest divisor's " - "update rate which is : %d Hz.", - controller_name.c_str(), controller_update_rate, cm_update_rate, act_ctrl_update_rate); + "The controller : %s update cycles won't be triggered at a constant period : %f sec, as the " + "controller's update rate : %d Hz is not a perfect divisor of the controller manager's " + "update rate : %d Hz!.", + controller_name.c_str(), 1.0 / controller_update_rate, controller_update_rate, + cm_update_rate); } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers @@ -1459,6 +1458,9 @@ void ControllerManager::activate_controllers( } auto controller = found_it->c; auto controller_name = found_it->info.name; + // reset the next update cycle time for newly activated controllers + *found_it->next_update_cycle_time = + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); bool assignment_successful = true; // assign command interfaces to the controller @@ -2031,19 +2033,24 @@ controller_interface::return_type ControllerManager::update( ++update_loop_counter_; update_loop_counter_ %= update_rate_; - for (auto loaded_controller : rt_controller_list) + for (const auto & loaded_controller : rt_controller_list) { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) { const auto controller_update_rate = loaded_controller.c->get_update_rate(); - const auto controller_update_factor = - (controller_update_rate == 0) || (controller_update_rate >= update_rate_) - ? 1u - : update_rate_ / controller_update_rate; + const bool run_controller_at_cm_rate = + (controller_update_rate == 0) || (controller_update_rate >= update_rate_); + const auto controller_period = + run_controller_at_cm_rate ? period + : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); + + bool controller_go = + (time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || + (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); - bool controller_go = ((update_loop_counter_ % controller_update_factor) == 0); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", update_loop_counter_, controller_go ? "True" : "False", @@ -2051,10 +2058,15 @@ controller_interface::return_type ControllerManager::update( if (controller_go) { - auto controller_ret = loaded_controller.c->update( - time, (controller_update_factor != 1u) - ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate) - : period); + auto controller_ret = loaded_controller.c->update(time, controller_period); + + if ( + *loaded_controller.next_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) + { + *loaded_controller.next_update_cycle_time = time; + } + *loaded_controller.next_update_cycle_time += controller_period; if (controller_ret != controller_interface::return_type::OK) { diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 78c3fcb06b..7bf5cae628 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -41,7 +41,6 @@ namespace { -const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; @@ -101,6 +100,7 @@ class ControllerManagerFixture : public ::testing::Test cm_->robot_description_callback(msg); } } + time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); } static void SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -119,7 +119,7 @@ class ControllerManagerFixture : public ::testing::Test { while (run_updater_) { - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }); @@ -157,6 +157,7 @@ class ControllerManagerFixture : public ::testing::Test bool run_updater_; const std::string robot_description_; const bool pass_urdf_as_parameter_; + rclcpp::Time time_; }; class TestControllerManagerSrvs @@ -177,9 +178,9 @@ class TestControllerManagerSrvs std::chrono::milliseconds(10), [&]() { - cm_->read(TIME, PERIOD); - cm_->update(TIME, PERIOD); - cm_->write(TIME, PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); executor_->add_node(cm_); @@ -206,7 +207,7 @@ class TestControllerManagerSrvs while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) != rclcpp::FutureReturnCode::SUCCESS) { - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); } } else diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 2c0c00d062..4821ec8d73 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -124,7 +124,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -136,7 +136,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) cm_->configure_controller(TEST_CONTROLLER2_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; @@ -151,7 +151,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -160,7 +160,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); // Start the real test controller, will take effect at the end of the update function @@ -175,7 +175,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -185,7 +185,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller->internal_counter, 1u); size_t last_internal_counter = test_controller->internal_counter; @@ -201,7 +201,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter) << "Controller is stopped at the end of update, so it should have done one more update"; { @@ -236,7 +236,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -248,7 +248,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -265,7 +265,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -279,7 +279,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); } EXPECT_GE(test_controller->internal_counter, 1u); EXPECT_EQ(test_controller->get_update_rate(), 4u); @@ -314,7 +314,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -327,7 +327,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -344,7 +344,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is started at the end of update"; { @@ -360,7 +360,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -376,8 +376,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "switch_controller should be blocking until next update cycle"; EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))) + controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))) << "Controller is stopped at the end of update, so it should have done one more update"; { ControllerManagerRunner cm_runner(this); @@ -421,7 +420,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -439,7 +438,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; { ControllerManagerRunner cm_runner(this); @@ -451,23 +450,28 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate); const auto cm_update_rate = cm_->get_update_rate(); const auto controller_update_rate = test_controller->get_update_rate(); - const auto controller_factor = (cm_update_rate / controller_update_rate); - const auto expected_controller_update_rate = - static_cast(std::round(cm_update_rate / static_cast(controller_factor))); const auto initial_counter = test_controller->internal_counter; - for (size_t update_counter = 1; update_counter <= 10 * cm_update_rate; ++update_counter) + rclcpp::Time time = time_; + for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time, rclcpp::Duration::from_seconds(0.01))); + time += rclcpp::Duration::from_seconds(0.01); if (update_counter % cm_update_rate == 0) { const auto no_of_secs_passed = update_counter / cm_update_rate; - EXPECT_EQ( + // NOTE: here EXPECT_NEAR is used because it is observed that in the first iteration of whole + // cycle of cm_update_rate counts, there is one count missing, but in rest of the 9 cycles it + // is clearly tracking, so adding 1 here won't affect the final count. + // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update + // cycle and then on accumulating 37 on every other update cycle so at the end of the 10 + // cycles it will have 369 instead of 370. + EXPECT_NEAR( test_controller->internal_counter - initial_counter, - (expected_controller_update_rate * no_of_secs_passed)); + (controller_update_rate * no_of_secs_passed), 1); } } } diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 1ec725f8c5..f12ab9e96b 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -22,6 +22,7 @@ #include "controller_manager_test_common.hpp" #include "hardware_interface/types/lifecycle_state_names.hpp" #include "lifecycle_msgs/msg/state.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" #include "test_controller/test_controller.hpp" using ::testing::_; @@ -138,7 +139,7 @@ class TestControllerManagerWithTestableCM cm_->configure_controller(TEST_BROADCASTER_ALL_NAME); cm_->configure_controller(TEST_BROADCASTER_SENSOR_NAME); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(0u, test_controller_actuator->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller_system->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_broadcaster_all->internal_counter) << "Controller is not started"; @@ -162,10 +163,6 @@ class TestControllerManagerWithTestableCM {}, strictness); } - // values to set to hardware to simulate failure on read and write - static constexpr double READ_FAIL_VALUE = 28282828.0; - static constexpr double WRITE_FAIL_VALUE = 23232323.0; - static constexpr char TEST_CONTROLLER_ACTUATOR_NAME[] = "test_controller_actuator"; static constexpr char TEST_CONTROLLER_SYSTEM_NAME[] = "test_controller_system"; static constexpr char TEST_BROADCASTER_ALL_NAME[] = "test_broadcaster_all"; @@ -226,7 +223,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -249,7 +246,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -258,10 +255,10 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to // READ_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -278,7 +275,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -289,7 +286,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -327,13 +324,13 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Simulate error in read() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME // by setting first command interface to READ_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; - test_controller_system->set_first_command_interface_value_to = READ_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::READ_FAIL_VALUE; { auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -350,7 +347,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er auto previous_counter_higher = test_controller_system->internal_counter; auto new_counter = test_broadcaster_sensor->internal_counter + 1; - EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -361,7 +358,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) @@ -418,7 +415,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -441,7 +438,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -450,10 +447,10 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME by setting first command interface to // WRITE_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -470,7 +467,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -481,7 +478,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -519,13 +516,13 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Simulate error in write() on TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME // by setting first command interface to WRITE_FAIL_VALUE - test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; - test_controller_system->set_first_command_interface_value_to = WRITE_FAIL_VALUE; + test_controller_actuator->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; + test_controller_system->set_first_command_interface_value_to = test_constants::WRITE_FAIL_VALUE; { auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -544,7 +541,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -555,7 +552,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has write error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 16ba39d953..a34d70ada7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -378,7 +378,7 @@ class TestControllerChainingWithControllerManager position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check if all controllers are updated diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index d67ae8a7d1..4137386f72 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -39,9 +39,9 @@ class TestLoadController : public ControllerManagerFixtureread(TIME, PERIOD); - cm_->update(TIME, PERIOD); - cm_->write(TIME, PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); update_executor_ = diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst index 8072652c55..ae3049ef44 100644 --- a/controller_manager_msgs/CHANGELOG.rst +++ b/controller_manager_msgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package controller_manager_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/controller_manager_msgs/package.xml b/controller_manager_msgs/package.xml index f7b9eb5642..b384585b22 100644 --- a/controller_manager_msgs/package.xml +++ b/controller_manager_msgs/package.xml @@ -2,7 +2,7 @@ controller_manager_msgs - 3.18.0 + 3.19.1 Messages and services for the controller manager. Bence Magyar Denis Štogl diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst index 0428cf4f8e..7c14eba8a6 100644 --- a/hardware_interface/CHANGELOG.rst +++ b/hardware_interface/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- +* [MockHardware] Added dynamic simulation functionality. (`#1028 `_) +* Add GPIO tag description to docs (`#1109 `_) +* Contributors: Christoph Fröhlich, Dr. Denis + 3.18.0 (2023-08-17) ------------------- diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2d6c72ffae..e486302c4c 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -101,7 +101,9 @@ if(BUILD_TESTING) test/test_components/test_system.cpp) target_link_libraries(test_components hardware_interface) ament_target_dependencies(test_components - pluginlib) + pluginlib + ros2_control_test_assets + ) install(TARGETS test_components DESTINATION lib ) diff --git a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp index 5c3ea22ca0..d39dee2c64 100644 --- a/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp +++ b/hardware_interface/include/hardware_interface/types/hardware_interface_return_values.hpp @@ -23,6 +23,7 @@ enum class return_type : std::uint8_t { OK = 0, ERROR = 1, + DEACTIVATE = 2, }; } // namespace hardware_interface diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index c244ee1254..e9b38de65d 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -32,6 +32,10 @@ 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: @@ -41,6 +45,14 @@ 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 @@ -60,8 +72,6 @@ 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; @@ -115,6 +125,9 @@ 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/package.xml b/hardware_interface/package.xml index e3a2f4f71e..c53c8523ec 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -1,7 +1,7 @@ hardware_interface - 3.18.0 + 3.19.1 ros2_control hardware interface Bence Magyar Denis Štogl diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 13a5a8b679..3ae3568a9e 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -131,6 +131,17 @@ 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_ = ""; @@ -312,7 +323,7 @@ std::vector GenericSystem::export_command_ std::vector command_interfaces; // Joints' state interfaces - for (auto i = 0u; i < info_.joints.size(); i++) + for (size_t i = 0; i < info_.joints.size(); ++i) { const auto & joint = info_.joints[i]; for (const auto & interface : joint.command_interfaces) @@ -333,6 +344,8 @@ 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_) @@ -369,7 +382,135 @@ std::vector GenericSystem::export_command_ return command_interfaces; } -return_type GenericSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +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) { if (command_propagation_disabled_) { @@ -392,19 +533,97 @@ 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 (!std::isnan(joint_commands_[POSITION_INTERFACE_INDEX][j])) + if (calculate_dynamics_) { - joint_states_[POSITION_INTERFACE_INDEX][j] = - joint_commands_[POSITION_INTERFACE_INDEX][j] + - (custom_interface_with_following_offset_.empty() ? position_state_following_offset_ : 0.0); + 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); + } + } } } - // do loopback on all other interfaces - starts from 1 because 0 index is position interface - mirror_command_to_state(joint_states_, joint_commands_, 1); + // 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); + } for (const auto & mimic_joint : mimic_joints_) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 73daddd23d..36ace2b78f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1259,12 +1259,24 @@ HardwareReadWriteStatus ResourceManager::read( { for (auto & component : components) { - if (component.read(time, period) != return_type::OK) + auto ret_val = component.read(time, period); + if (ret_val == return_type::ERROR) { read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } + // If desired: automatic re-activation. We could add a flag for this... + // else + // { + // using lifecycle_msgs::msg::State; + // rclcpp_lifecycle::State state(State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE); + // set_component_state(component.get_name(), state); + // } } }; @@ -1287,12 +1299,17 @@ HardwareReadWriteStatus ResourceManager::write( { for (auto & component : components) { - if (component.write(time, period) != return_type::OK) + auto ret_val = component.write(time, period); + if (ret_val == return_type::ERROR) { read_write_status.ok = false; read_write_status.failed_hardware_names.push_back(component.get_name()); resource_storage_->remove_all_hardware_interfaces_from_available_list(component.get_name()); } + else if (ret_val == return_type::DEACTIVATE) + { + resource_storage_->deactivate_hardware(component); + } } }; diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 8e78ade7fb..cd344b600e 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -33,7 +33,8 @@ namespace { const auto TIME = rclcpp::Time(0); -const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +const auto PERIOD = rclcpp::Duration::from_seconds(0.1); // 0.1 seconds for easier math +const auto COMPARE_DELTA = 0.0001; } // namespace class TestGenericSystem : public ::testing::Test @@ -484,6 +485,39 @@ 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"( @@ -521,6 +555,7 @@ 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_; }; @@ -1624,6 +1659,202 @@ 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); + + // 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 = @@ -1641,7 +1872,6 @@ 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"); diff --git a/hardware_interface/test/test_components/test_actuator.cpp b/hardware_interface/test/test_components/test_actuator.cpp index 5f9c09e95e..7cf55b50d3 100644 --- a/hardware_interface/test/test_components/test_actuator.cpp +++ b/hardware_interface/test/test_components/test_actuator.cpp @@ -16,6 +16,7 @@ #include #include "hardware_interface/actuator_interface.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::ActuatorInterface; using hardware_interface::CommandInterface; @@ -76,12 +77,17 @@ class TestActuator : public ActuatorInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_ == 28282828.0) + if (velocity_command_ == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } + // simulate deactivate on read + if (velocity_command_ == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } // The next line is for the testing purposes. We need value to be changed to be sure that // the feedback from hardware to controllers in the chain is working as it should. // This makes value checks clearer and confirms there is no "state = command" line or some @@ -93,12 +99,17 @@ class TestActuator : public ActuatorInterface return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_ == 23232323.0) + if (velocity_command_ == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_ = 0.0; return return_type::ERROR; } + // simulate deactivate on write + if (velocity_command_ == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } diff --git a/hardware_interface/test/test_components/test_system.cpp b/hardware_interface/test/test_components/test_system.cpp index 7477734cff..3326cb893b 100644 --- a/hardware_interface/test/test_components/test_system.cpp +++ b/hardware_interface/test/test_components/test_system.cpp @@ -18,6 +18,7 @@ #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using hardware_interface::CommandInterface; using hardware_interface::return_type; @@ -81,24 +82,34 @@ class TestSystem : public SystemInterface return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on read - if (velocity_command_[0] == 28282828) + if (velocity_command_[0] == test_constants::READ_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } + // simulate deactivate on read + if (velocity_command_[0] == test_constants::READ_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { // simulate error on write - if (velocity_command_[0] == 23232323) + if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE) { // reset value to get out from error on the next call - simplifies CM tests velocity_command_[0] = 0.0; return return_type::ERROR; } + // simulate deactivate on write + if (velocity_command_[0] == test_constants::WRITE_DEACTIVATE_VALUE) + { + return return_type::DEACTIVATE; + } return return_type::OK; } diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index aedf2b862d..c80cdbb5bf 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -28,6 +28,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp_lifecycle/state.hpp" #include "ros2_control_test_assets/descriptions.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; @@ -1538,6 +1539,122 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest } } + void check_read_or_write_deactivate( + FunctionT method_that_deactivates, FunctionT other_method, const double deactivate_value) + { + // define state to set components to + rclcpp_lifecycle::State state_active( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + hardware_interface::lifecycle_state_names::ACTIVE); + + // deactivate for TEST_ACTUATOR_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value - 10.0); + { + // deactivate on error + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + + // reactivate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate for TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value - 10.0); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + + // deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + claimed_itfs[0].set_value(deactivate_value); + claimed_itfs[1].set_value(deactivate_value); + { + // deactivate on flag + auto [ok, failed_hardware_names] = method_that_deactivates(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + check_if_interface_available(true, true); + // re-activate + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active); + status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + check_if_interface_available(true, true); + } + // write is sill OK + { + auto [ok, failed_hardware_names] = other_method(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + check_if_interface_available(true, true); + } + } + public: std::shared_ptr rm; std::vector claimed_itfs; @@ -1546,8 +1663,6 @@ class ResourceManagerTestReadWriteError : public ResourceManagerTest const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); // values to set to hardware to simulate failure on read and write - static constexpr double READ_FAIL_VALUE = 28282828.0; - static constexpr double WRITE_FAIL_VALUE = 23232323.0; }; TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) @@ -1558,7 +1673,7 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_read) // check read methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::read, rm, _1, _2), - std::bind(&TestableResourceManager::write, rm, _1, _2), READ_FAIL_VALUE); + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_FAIL_VALUE); } TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) @@ -1569,7 +1684,29 @@ TEST_F(ResourceManagerTestReadWriteError, handle_error_on_hardware_write) // check write methods failures check_read_or_write_failure( std::bind(&TestableResourceManager::write, rm, _1, _2), - std::bind(&TestableResourceManager::read, rm, _1, _2), WRITE_FAIL_VALUE); + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_FAIL_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_read) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check read methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::read, rm, _1, _2), + std::bind(&TestableResourceManager::write, rm, _1, _2), test_constants::READ_DEACTIVATE_VALUE); +} + +TEST_F(ResourceManagerTestReadWriteError, handle_deactivate_on_hardware_write) +{ + setup_resource_manager_and_do_initial_checks(); + + using namespace std::placeholders; + // check write methods failures + check_read_or_write_deactivate( + std::bind(&TestableResourceManager::write, rm, _1, _2), + std::bind(&TestableResourceManager::read, rm, _1, _2), test_constants::WRITE_DEACTIVATE_VALUE); } TEST_F(ResourceManagerTest, test_caching_of_controllers_to_hardware) diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst index 46acee3f8d..c994efe036 100644 --- a/joint_limits/CHANGELOG.rst +++ b/joint_limits/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package joint_limits ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/joint_limits/package.xml b/joint_limits/package.xml index f1c0365cfd..fc2cd26f9e 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -1,6 +1,6 @@ joint_limits - 3.18.0 + 3.19.1 Interfaces for handling of joint limits for controllers or hardware. Bence Magyar diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst index 272c2d3fe4..5032992acf 100644 --- a/ros2_control/CHANGELOG.rst +++ b/ros2_control/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2_control/package.xml b/ros2_control/package.xml index 15a379933a..ab9112ad81 100644 --- a/ros2_control/package.xml +++ b/ros2_control/package.xml @@ -1,7 +1,7 @@ ros2_control - 3.18.0 + 3.19.1 Metapackage for ROS2 control related packages Bence Magyar Denis Štogl diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst index d633c415d1..d1305ad796 100644 --- a/ros2_control_test_assets/CHANGELOG.rst +++ b/ros2_control_test_assets/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_control_test_assets ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp new file mode 100644 index 0000000000..eb2bbf24c7 --- /dev/null +++ b/ros2_control_test_assets/include/ros2_control_test_assets/test_hardware_interface_constants.hpp @@ -0,0 +1,28 @@ +// Copyright (c) 2023, FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +/// \author: Felix Exner + +#ifndef ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +#define ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ +namespace test_constants +{ +/// Constants defining special values used inside tests to trigger things like deactivate or errors +/// on read/write. +constexpr double READ_FAIL_VALUE = 28282828.0; +constexpr double WRITE_FAIL_VALUE = 23232323.0; +constexpr double READ_DEACTIVATE_VALUE = 29292929.0; +constexpr double WRITE_DEACTIVATE_VALUE = 24242424.0; +} // namespace test_constants +#endif // ROS2_CONTROL_TEST_ASSETS__TEST_HARDWARE_INTERFACE_CONSTANTS_HPP_ diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/package.xml index c77b828276..ccf21c5295 100644 --- a/ros2_control_test_assets/package.xml +++ b/ros2_control_test_assets/package.xml @@ -2,7 +2,7 @@ ros2_control_test_assets - 3.18.0 + 3.19.1 The package provides shared test resources for ros2_control stack Bence Magyar diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst index a5a153faf1..ba85f2a928 100644 --- a/ros2controlcli/CHANGELOG.rst +++ b/ros2controlcli/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2controlcli ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index 5880c054cb..5eada2acb6 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -148,7 +148,7 @@ load_controller .. code-block:: console $ ros2 control load_controller -h - usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set_state {configure,activate}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name + usage: ros2 control load_controller [-h] [--spin-time SPIN_TIME] [--set-state {inactive,active}] [-c CONTROLLER_MANAGER] [--include-hidden-nodes] controller_name Load a controller in a controller manager @@ -159,7 +159,7 @@ load_controller -h, --help show this help message and exit --spin-time SPIN_TIME Spin time in seconds to wait for discovery (only applies when not using an already running daemon) - --set_state {inactive,active} + --set-state {inactive,active} Set the state of the loaded controller -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index 92eba51b62..d91f66d4ea 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -2,7 +2,7 @@ ros2controlcli - 3.18.0 + 3.19.1 The ROS 2 command line tools for ROS2 Control. diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index aa2b19b830..fb5c06e6f4 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version="3.18.0", + version="3.19.1", packages=find_packages(exclude=["test"]), data_files=[ ("share/" + package_name, ["package.xml"]), diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst index 9996ff5a85..5fd421b0cb 100644 --- a/rqt_controller_manager/CHANGELOG.rst +++ b/rqt_controller_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rqt_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/rqt_controller_manager/package.xml b/rqt_controller_manager/package.xml index 4888459d49..3f56fd18b6 100644 --- a/rqt_controller_manager/package.xml +++ b/rqt_controller_manager/package.xml @@ -2,7 +2,7 @@ rqt_controller_manager - 3.18.0 + 3.19.1 Graphical frontend for interacting with the controller manager. Bence Magyar Denis Štogl diff --git a/rqt_controller_manager/setup.py b/rqt_controller_manager/setup.py index 318cf556d2..001968714a 100644 --- a/rqt_controller_manager/setup.py +++ b/rqt_controller_manager/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="3.18.0", + version="3.19.1", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/transmission_interface/CHANGELOG.rst b/transmission_interface/CHANGELOG.rst index c45b040c1b..f507b8921f 100644 --- a/transmission_interface/CHANGELOG.rst +++ b/transmission_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package transmission_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.19.1 (2023-10-04) +------------------- + +3.19.0 (2023-10-03) +------------------- + 3.18.0 (2023-08-17) ------------------- diff --git a/transmission_interface/package.xml b/transmission_interface/package.xml index 9e2b2fded2..4d68170366 100644 --- a/transmission_interface/package.xml +++ b/transmission_interface/package.xml @@ -2,7 +2,7 @@ transmission_interface - 3.18.0 + 3.19.1 transmission_interface contains data structures for representing mechanical transmissions, methods for propagating values between actuator and joint spaces and tooling to support this. Bence Magyar Denis Štogl