diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6fadbbdace..205e0f63ab 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -37,7 +37,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.17.0 + rev: v3.19.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -50,20 +50,20 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/psf/black - rev: 24.4.2 + rev: 24.10.0 hooks: - id: black args: ["--line-length=99"] - repo: https://github.com/pycqa/flake8 - rev: 7.1.0 + rev: 7.1.1 hooks: - id: flake8 args: ["--extend-ignore=E501"] # CPP hooks - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.3 hooks: - id: clang-format args: ['-fallback-style=none', '-i'] @@ -109,7 +109,7 @@ repos: # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.1 + rev: v1.1.2 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -133,7 +133,7 @@ repos: exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$ - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.1 + rev: 0.29.4 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index e4ea6ceba8..bc2997b8a9 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index a191cede5d..5764de56c1 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.13.0 + 4.15.0 Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. Apache License 2.0 Bence Magyar diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index c04e87be95..c434f73467 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -89,9 +89,9 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 96dd20d80e..4f67beaa07 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -91,9 +91,9 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 3f0f44ebde..46899f9cd6 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- * Fix segfault at reconfigure of AdmittanceController (`#1248 `_) diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index bd37557b06..06fde3cb59 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.13.0 + 4.15.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index 9de6109a7e..0d096951ca 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 73b658220a..f58cf42dff 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.13.0 + 4.15.0 Steering controller with bicycle kinematics. Rear fixed wheel is powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 573992b24e..7ec0983f93 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -73,9 +73,9 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 0bc03f4886..0a321009c2 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -77,9 +77,9 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index e82c7ee4f4..4c48fb234d 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index d352de7ed4..45ab80d5cb 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.13.0 + 4.15.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 683f23c202..d5b956aa8a 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -71,6 +71,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> + Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> Common Controller Parameters diff --git a/doc/release_notes.rst b/doc/release_notes.rst index b55db861b3..688c35724d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -48,6 +48,8 @@ joint_trajectory_controller * -1 - The tolerance is "erased". If there was a default, the joint will be allowed to move without restriction. +* Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). + pid_controller ************************ * 🚀 The PID controller was added 🎉 (`#434 `_). @@ -56,6 +58,7 @@ steering_controllers_library ******************************** * Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 `_). * A fix for Ackermann steering odometry was added (`#921 `_). +* Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). tricycle_controller ************************ diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 573f951c1b..ea2efdcf4e 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 3ec600b0fa..fab6d671ad 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.13.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/force_torque_sensor_broadcaster/CHANGELOG.rst b/force_torque_sensor_broadcaster/CHANGELOG.rst index d8ee1d38d1..7d2a7a1a8a 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml index 10d19a93c5..8e61e013de 100644 --- a/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml +++ b/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.xml @@ -1,6 +1,6 @@ + type="force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster" base_class_type="controller_interface::ChainableControllerInterface"> This controller publishes the readings of force-torque interfaces as geometry_msgs/WrenchStamped message. diff --git a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp index bd477ed68a..e5a5349c32 100644 --- a/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp +++ b/force_torque_sensor_broadcaster/include/force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp @@ -20,8 +20,9 @@ #define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_ #include +#include -#include "controller_interface/controller_interface.hpp" +#include "controller_interface/chainable_controller_interface.hpp" #include "force_torque_sensor_broadcaster/visibility_control.h" // auto-generated by generate_parameter_library #include "force_torque_sensor_broadcaster_parameters.hpp" @@ -32,7 +33,7 @@ namespace force_torque_sensor_broadcaster { -class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface +class ForceTorqueSensorBroadcaster : public controller_interface::ChainableControllerInterface { public: FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC @@ -59,10 +60,19 @@ class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInte const rclcpp_lifecycle::State & previous_state) override; FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC - controller_interface::return_type update( + controller_interface::return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) override; + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC + std::vector on_export_state_interfaces() override; + protected: + void apply_sensor_offset(const Params & params, geometry_msgs::msg::WrenchStamped & msg); + std::shared_ptr param_listener_; Params params_; diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index a41568d2d5..1383342246 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.13.0 + 4.15.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp index 9b570d353f..ae105a511c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp @@ -24,7 +24,7 @@ namespace force_torque_sensor_broadcaster { ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster() -: controller_interface::ControllerInterface() +: controller_interface::ChainableControllerInterface() { } @@ -141,23 +141,97 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::return_type ForceTorqueSensorBroadcaster::update( +controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + } if (realtime_publisher_ && realtime_publisher_->trylock()) { realtime_publisher_->msg_.header.stamp = time; force_torque_sensor_->get_values_as_message(realtime_publisher_->msg_.wrench); + this->apply_sensor_offset(params_, realtime_publisher_->msg_); realtime_publisher_->unlockAndPublish(); } return controller_interface::return_type::OK; } +controller_interface::return_type ForceTorqueSensorBroadcaster::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +std::vector +ForceTorqueSensorBroadcaster::on_export_state_interfaces() +{ + std::vector exported_state_interfaces; + + std::vector force_names( + {params_.interface_names.force.x, params_.interface_names.force.y, + params_.interface_names.force.z}); + std::vector torque_names( + {params_.interface_names.torque.x, params_.interface_names.torque.y, + params_.interface_names.torque.z}); + if (!params_.sensor_name.empty()) + { + const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names(); + std::copy( + semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin()); + std::copy( + semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin()); + } + const std::string controller_name = get_node()->get_name(); + if (!force_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x)); + } + if (!force_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y)); + } + if (!force_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z)); + } + if (!torque_names[0].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x)); + } + if (!torque_names[1].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y)); + } + if (!torque_names[2].empty()) + { + exported_state_interfaces.emplace_back(hardware_interface::StateInterface( + controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z)); + } + return exported_state_interfaces; +} + +void ForceTorqueSensorBroadcaster::apply_sensor_offset( + const Params & params, geometry_msgs::msg::WrenchStamped & msg) +{ + msg.wrench.force.x += params.offset.force.x; + msg.wrench.force.y += params.offset.force.y; + msg.wrench.force.z += params.offset.force.z; + msg.wrench.torque.x += params.offset.torque.x; + msg.wrench.torque.y += params.offset.torque.y; + msg.wrench.torque.z += params.offset.torque.z; +} } // namespace force_torque_sensor_broadcaster #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster, - controller_interface::ControllerInterface) + controller_interface::ChainableControllerInterface) diff --git a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml index 3e75ab6012..0869f5cf3c 100644 --- a/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml +++ b/force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster_parameters.yaml @@ -46,3 +46,36 @@ force_torque_sensor_broadcaster: default_value: "", description: "Name of the state interface with torque values around 'z' axis.", } + offset: + force: + x: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of force values on 'z' axis.", + } + torque: + x: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'x' axis.", + } + y: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'y' axis.", + } + z: { + type: double, + default_value: 0.0, + description: "The offset of torque values around 'z' axis.", + } diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp index 1ea25520cc..e436beb2e5 100644 --- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp @@ -278,6 +278,60 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); } +TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets) +{ + SetUpFTSBroadcaster(); + + std::array force_offsets = {10.0, 30.0, -50.0}; + std::array torque_offsets = {1.0, -1.2, -5.2}; + // set the params 'sensor_name' and 'frame_id' + fts_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_}); + fts_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.x", force_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.y", force_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.force.z", force_offsets[2]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.x", torque_offsets[0]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.y", torque_offsets[1]}); + fts_broadcaster_->get_node()->set_parameter({"offset.torque.z", torque_offsets[2]}); + + ASSERT_EQ(fts_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(fts_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + geometry_msgs::msg::WrenchStamped wrench_msg; + subscribe_and_get_message(wrench_msg); + + ASSERT_EQ(wrench_msg.header.frame_id, frame_id_); + ASSERT_EQ(wrench_msg.wrench.force.x, sensor_values_[0] + force_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.force.y, sensor_values_[1] + force_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.force.z, sensor_values_[2] + force_offsets[2]); + ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3] + torque_offsets[0]); + ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4] + torque_offsets[1]); + ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5] + torque_offsets[2]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ( + exported_state_interfaces[0]->get_name(), controller_name + "/" + sensor_name_ + "/force.x"); + ASSERT_EQ( + exported_state_interfaces[1]->get_name(), controller_name + "/" + sensor_name_ + "/force.y"); + ASSERT_EQ( + exported_state_interfaces[2]->get_name(), controller_name + "/" + sensor_name_ + "/force.z"); + ASSERT_EQ( + exported_state_interfaces[3]->get_name(), controller_name + "/" + sensor_name_ + "/torque.x"); + ASSERT_EQ( + exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y"); + ASSERT_EQ( + exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ( + exported_state_interfaces[i]->get_value(), + sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3])); + } +} + TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) { SetUpFTSBroadcaster(); @@ -300,6 +354,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success) ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.x)); ASSERT_TRUE(std::isnan(wrench_msg.wrench.torque.y)); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 2u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z"); + ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]); + ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]); } TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) @@ -328,6 +391,21 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success) ASSERT_EQ(wrench_msg.wrench.torque.x, sensor_values_[3]); ASSERT_EQ(wrench_msg.wrench.torque.y, sensor_values_[4]); ASSERT_EQ(wrench_msg.wrench.torque.z, sensor_values_[5]); + + // Check the exported state interfaces + const auto exported_state_interfaces = fts_broadcaster_->export_state_interfaces(); + ASSERT_EQ(exported_state_interfaces.size(), 6u); + const std::string controller_name = fts_broadcaster_->get_node()->get_name(); + ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x"); + ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/force.y"); + ASSERT_EQ(exported_state_interfaces[2]->get_name(), controller_name + "/fts_sensor/force.z"); + ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x"); + ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y"); + ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z"); + for (size_t i = 0; i < 6; ++i) + { + ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]); + } } int main(int argc, char ** argv) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index 04ce12659e..54caa9d764 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 7bef16d4fd..f9f30dd053 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.13.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 8a22013b50..11c61f86cf 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index 65d8c58e0f..d46c98f608 100644 --- a/gripper_controllers/package.xml +++ b/gripper_controllers/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> gripper_controllers - 4.13.0 + 4.15.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index 1d357eb88f..40af7a5360 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index c4e31db09f..ed1f794afa 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.13.0 + 4.15.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 3b61fbec71..cf422e3eb8 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* [JSB] Move the initialize of urdf::Model from on_activate to on_configure to improve real-time performance (`#1269 `_) +* Contributors: Takashi Sato + 4.13.0 (2024-08-22) ------------------- * [Joint State Broadcaster] Publish the joint_states of joints present in the URDF (`#1233 `_) diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index 121ca68f6a..c3a1e66574 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.13.0 + 4.15.0 Broadcaster to publish joint state Bence Magyar Denis Stogl diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index fe0b32213a..9bbb862925 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -209,6 +209,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { joint_names_.clear(); + name_if_value_mapping_.clear(); return CallbackReturn::SUCCESS; } @@ -309,6 +310,8 @@ void JointStateBroadcaster::init_joint_state_msg() void JointStateBroadcaster::init_dynamic_joint_state_msg() { auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_; + dynamic_joint_state_msg.joint_names.clear(); + dynamic_joint_state_msg.interface_values.clear(); for (const auto & name_ifv : name_if_value_mapping_) { const auto & name = name_ifv.first; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 877d199419..de534c00b6 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -216,6 +216,102 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest) ElementsAreArray(interface_names_)); } +TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest) +{ + // publishers not initialized yet + ASSERT_FALSE(state_broadcaster_->joint_state_publisher_); + ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_); + + SetUpStateBroadcaster(); + // configure ok + ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS = joint_names_.size(); + + // check interface configuration + auto cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + auto state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_)); + ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS)); + ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS)); + + // dynamic joint state initialized + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS)); + ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); + + // Now deactivate and activate with only 2 set of joints and interfaces (to create as in one of + // the interface is unavailable) + ASSERT_EQ(state_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + const std::vector JOINT_NAMES = {"joint1", "joint2"}; + assign_state_interfaces(JOINT_NAMES, interface_names_); + + ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + const size_t NUM_JOINTS_WITH_ONE_DEACTIVATED = JOINT_NAMES.size(); + + // check interface configuration + cmd_if_conf = state_broadcaster_->command_interface_configuration(); + ASSERT_THAT(cmd_if_conf.names, IsEmpty()); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE); + state_if_conf = state_broadcaster_->state_interface_configuration(); + ASSERT_THAT(state_if_conf.names, IsEmpty()); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL); + + // publishers initialized + ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_); + ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_); + + // joint state initialized + const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_; + ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_joint_state_msg.effort, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + + // dynamic joint state initialized + const auto & new_dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED)); + ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[0].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[1].interface_names, + ElementsAreArray(interface_names_)); + ASSERT_THAT( + new_dynamic_joint_state_msg.interface_values[2].interface_names, + ElementsAreArray(interface_names_)); +} + TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) { const std::vector JOINT_NAMES = {}; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 0b4c50e89e..4f4241d3d7 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -34,6 +34,7 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr { FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); + FRIEND_TEST(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription); diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 9ba9778c37..81696246b1 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + 4.13.0 (2024-08-22) ------------------- diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 8c556703ab..4902fd1dcc 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -133,7 +133,7 @@ double resolve_tolerance_source(const double default_value, const double goal_va // * -1 - The tolerance is "erased". // If there was a default, the joint will be allowed to move without restriction. constexpr double ERASE_VALUE = -1.0; - auto is_erase_value = [](double value) + auto is_erase_value = [=](double value) { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; if (goal_value > 0.0) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 9ff1dd03c9..112c4ad9a1 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.13.0 + 4.15.0 Controller for executing joint-space trajectories on a group of joints Bence Magyar Dr. Denis Štogl diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e4923604fd..bea37e02be 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -924,7 +924,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); - if (read_state_from_command_interfaces(state)) + if ( + params_.set_last_command_interface_value_as_state_on_activation && + read_state_from_command_interfaces(state)) { state_current_ = state; last_commanded_state_ = state; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 8bd64b6314..14b71f0711 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -64,6 +64,11 @@ joint_trajectory_controller: default_value: false, description: "Allow integration in goal trajectories to accept goals without position or velocity specified", } + set_last_command_interface_value_as_state_on_activation: { + type: bool, + default_value: true, + description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. When set to false, the current state is used for both.", + } action_monitor_rate: { type: double, default_value: 20.0, diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 5a5a872dff..a3024fbf62 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 82d38cc417..b396ba7b85 100644 --- a/parallel_gripper_controller/package.xml +++ b/parallel_gripper_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> parallel_gripper_controller - 4.13.0 + 4.15.0 The parallel_gripper_controller package Bence Magyar diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index 02a10e05db..5a27471199 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + +4.14.0 (2024-09-11) +------------------- +* [PID Controller] Export state interfaces for easier chaining with other controllers (`#1214 `_) +* Contributors: Sai Kishor Kothakota + 4.13.0 (2024-08-22) ------------------- diff --git a/pid_controller/CMakeLists.txt b/pid_controller/CMakeLists.txt index 6c9e00ef8b..f9a9abce89 100644 --- a/pid_controller/CMakeLists.txt +++ b/pid_controller/CMakeLists.txt @@ -5,6 +5,16 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow -Werror=format) endif() +if(WIN32) + add_compile_definitions( + # For math constants + _USE_MATH_DEFINES + # Minimize Windows namespace collision + NOMINMAX + WIN32_LEAN_AND_MEAN + ) +endif() + set(THIS_PACKAGE_INCLUDE_DEPENDS angles control_msgs diff --git a/pid_controller/include/pid_controller/pid_controller.hpp b/pid_controller/include/pid_controller/pid_controller.hpp index 236b067929..ce66fd06d4 100644 --- a/pid_controller/include/pid_controller/pid_controller.hpp +++ b/pid_controller/include/pid_controller/pid_controller.hpp @@ -117,6 +117,8 @@ class PidController : public controller_interface::ChainableControllerInterface // override methods from ChainableControllerInterface std::vector on_export_reference_interfaces() override; + std::vector on_export_state_interfaces() override; + bool on_set_chained_mode(bool chained_mode) override; // internal methods diff --git a/pid_controller/package.xml b/pid_controller/package.xml index 767e5eecbc..79a66d3cc3 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.13.0 + 4.15.0 Controller based on PID implememenation from control_toolbox package. Bence Magyar Denis Štogl diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index c8e6cc0fe0..032dc1d666 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -187,6 +187,43 @@ controller_interface::CallbackReturn PidController::on_configure( auto measured_state_callback = [&](const std::shared_ptr state_msg) -> void { + if (state_msg->dof_names.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data names (%zu) is not matching the expected size (%zu).", + state_msg->dof_names.size(), reference_and_state_dof_names_.size()); + return; + } + if (state_msg->values.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data values (%zu) is not matching the expected size (%zu).", + state_msg->values.size(), reference_and_state_dof_names_.size()); + return; + } + + if (!state_msg->values_dot.empty()) + { + if (params_.reference_and_state_interfaces.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "The reference_and_state_interfaces parameter has to have two interfaces [the " + "interface and the derivative of the interface], in order to use the values_dot " + "field."); + return; + } + if (state_msg->values_dot.size() != reference_and_state_dof_names_.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of input data values_dot (%zu) is not matching the expected size (%zu).", + state_msg->values_dot.size(), reference_and_state_dof_names_.size()); + return; + } + } // TODO(destogl): Sort the input values based on joint and interface names measured_state_.writeFromNonRT(state_msg); }; @@ -363,6 +400,27 @@ std::vector PidController::on_export_refer return reference_interfaces; } +std::vector PidController::on_export_state_interfaces() +{ + std::vector state_interfaces; + state_interfaces.reserve(state_interfaces_values_.size()); + + state_interfaces_values_.resize( + reference_and_state_dof_names_.size() * params_.reference_and_state_interfaces.size(), + std::numeric_limits::quiet_NaN()); + size_t index = 0; + for (const auto & interface : params_.reference_and_state_interfaces) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + state_interfaces.push_back(hardware_interface::StateInterface( + get_node()->get_name(), dof_name + "/" + interface, &state_interfaces_values_[index])); + ++index; + } + } + return state_interfaces; +} + bool PidController::on_set_chained_mode(bool chained_mode) { // Always accept switch to/from chained mode @@ -438,6 +496,12 @@ controller_interface::return_type PidController::update_and_write_commands( } } + // Fill the information of the exported state interfaces + for (size_t i = 0; i < measured_state_values_.size(); ++i) + { + state_interfaces_values_[i] = measured_state_values_[i]; + } + for (size_t i = 0; i < dof_; ++i) { double tmp_command = std::numeric_limits::quiet_NaN(); diff --git a/pid_controller/src/pid_controller.yaml b/pid_controller/src/pid_controller.yaml index f645738862..651cc1e7de 100644 --- a/pid_controller/src/pid_controller.yaml +++ b/pid_controller/src/pid_controller.yaml @@ -34,6 +34,7 @@ pid_controller: read_only: true, validation: { not_empty<>: null, + size_gt<>: 0, size_lt<>: 3, } } diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp index 9b53dccb23..60d484e463 100644 --- a/pid_controller/test/test_pid_controller.cpp +++ b/pid_controller/test/test_pid_controller.cpp @@ -97,9 +97,9 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); ++ri_index; } } diff --git a/pid_controller/test/test_pid_controller.hpp b/pid_controller/test/test_pid_controller.hpp index 158b5d9147..4471f35a12 100644 --- a/pid_controller/test/test_pid_controller.hpp +++ b/pid_controller/test/test_pid_controller.hpp @@ -71,6 +71,7 @@ class TestablePidController : public pid_controller::PidController const rclcpp_lifecycle::State & previous_state) override { auto ref_itfs = on_export_reference_interfaces(); + auto state_itfs = on_export_state_interfaces(); return pid_controller::PidController::on_activate(previous_state); } diff --git a/pid_controller/test/test_pid_controller_preceding.cpp b/pid_controller/test/test_pid_controller_preceding.cpp index 498ca633da..ee5fe46754 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -84,12 +84,30 @@ TEST_F(PidControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; - EXPECT_EQ(ref_if_conf[ri_index].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[ri_index].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[ri_index].get_interface_name(), dof_name + "/" + interface); + EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface); ++ri_index; } } + + // check exported state itfs + auto exported_state_itfs = controller_->export_state_interfaces(); + ASSERT_EQ(exported_state_itfs.size(), dof_state_values_.size()); + size_t esi_index = 0; + for (const auto & interface : state_interfaces_) + { + for (const auto & dof_name : reference_and_state_dof_names_) + { + const std::string state_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface; + EXPECT_EQ(exported_state_itfs[esi_index]->get_name(), state_itf_name); + EXPECT_EQ( + exported_state_itfs[esi_index]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(exported_state_itfs[esi_index]->get_interface_name(), dof_name + "/" + interface); + ++esi_index; + } + } } int main(int argc, char ** argv) diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..46028cf258 --- /dev/null +++ b/pose_broadcaster/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.16) +project(pose_broadcaster + LANGUAGES + CXX +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(pose_broadcaster_parameters + src/pose_broadcaster_parameters.yaml +) + +add_library(pose_broadcaster SHARED + src/pose_broadcaster.cpp +) +target_compile_features(pose_broadcaster PUBLIC + cxx_std_17 +) +target_include_directories(pose_broadcaster PUBLIC + $ + $ +) +target_link_libraries(pose_broadcaster PUBLIC + pose_broadcaster_parameters +) +ament_target_dependencies(pose_broadcaster PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface pose_broadcaster.xml +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_pose_broadcaster + test/test_load_pose_broadcaster.cpp + ) + target_link_libraries(test_load_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_load_pose_broadcaster + controller_manager + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_pose_broadcaster + test/test_pose_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pose_broadcaster_params.yaml + ) + target_link_libraries(test_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_pose_broadcaster + hardware_interface + ) +endif() + +install( + DIRECTORY + include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS + pose_broadcaster + pose_broadcaster_parameters + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pose_broadcaster/README.md b/pose_broadcaster/README.md new file mode 100644 index 0000000000..bf47411048 --- /dev/null +++ b/pose_broadcaster/README.md @@ -0,0 +1,8 @@ +pose_broadcaster +========================================== + +Controller to publish poses provided by pose sensors. + +Pluginlib-Library: pose_broadcaster + +Plugin: pose_broadcaster/PoseBroadcaster (controller_interface::ControllerInterface) diff --git a/pose_broadcaster/doc/userdoc.rst b/pose_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..0ae40e2fad --- /dev/null +++ b/pose_broadcaster/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/doc/userdoc.rst + +.. _pose_broadcaster_userdoc: + +Pose Broadcaster +-------------------------------- +Broadcaster for poses measured by a robot or a sensor. +Poses are published as ``geometry_msgs/msg/PoseStamped`` messages and optionally as tf transforms. + +The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/pose_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/pose_broadcaster_params.yaml + :language: yaml diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp new file mode 100644 index 0000000000..621a90cc85 --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 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. +#ifndef POSE_BROADCASTER__POSE_BROADCASTER_HPP_ +#define POSE_BROADCASTER__POSE_BROADCASTER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "pose_broadcaster/visibility_control.h" +#include "pose_broadcaster_parameters.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.h" +#include "semantic_components/pose_sensor.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace pose_broadcaster +{ + +class PoseBroadcaster : public controller_interface::ControllerInterface +{ +public: + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::shared_ptr param_listener_; + Params params_; + + std::unique_ptr pose_sensor_; + + rclcpp::Publisher::SharedPtr pose_publisher_; + std::unique_ptr> + realtime_publisher_; + + std::optional tf_publish_period_; + rclcpp::Time tf_last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; + rclcpp::Publisher::SharedPtr tf_publisher_; + std::unique_ptr> + realtime_tf_publisher_; +}; + +} // namespace pose_broadcaster + +#endif // POSE_BROADCASTER__POSE_BROADCASTER_HPP_ diff --git a/pose_broadcaster/include/pose_broadcaster/visibility_control.h b/pose_broadcaster/include/pose_broadcaster/visibility_control.h new file mode 100644 index 0000000000..5ce272658d --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2024 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. + +#ifndef POSE_BROADCASTER__VISIBILITY_CONTROL_H_ +#define POSE_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define POSE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define POSE_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define POSE_BROADCASTER_EXPORT __declspec(dllexport) +#define POSE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef POSE_BROADCASTER_BUILDING_DLL +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT +#else +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#else +#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define POSE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // POSE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml new file mode 100644 index 0000000000..4c3506f1cd --- /dev/null +++ b/pose_broadcaster/package.xml @@ -0,0 +1,31 @@ + + + + pose_broadcaster + 0.0.0 + Broadcaster to publish cartesian states. + Robert Wilbrandt + + Apache License 2.0 + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2_msgs + + ament_cmake_gmock + controller_manager + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/pose_broadcaster/pose_broadcaster.xml b/pose_broadcaster/pose_broadcaster.xml new file mode 100644 index 0000000000..6578958004 --- /dev/null +++ b/pose_broadcaster/pose_broadcaster.xml @@ -0,0 +1,9 @@ + + + + This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message and optionally as a tf transform. + + + diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp new file mode 100644 index 0000000000..7e3aeaddf3 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -0,0 +1,195 @@ +// Copyright 2024 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. +#include "pose_broadcaster/pose_broadcaster.hpp" + +namespace +{ + +constexpr auto DEFAULT_POSE_TOPIC = "~/pose"; +constexpr auto DEFAULT_TF_TOPIC = "/tf"; + +} // namespace + +namespace pose_broadcaster +{ + +controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names = pose_sensor_->get_state_interface_names(); + + return state_interfaces_config; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_init() +{ + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & ex) + { + fprintf(stderr, "Exception thrown during init stage with message: %s\n", ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + pose_sensor_ = std::make_unique(params_.pose_name); + tf_publish_period_ = + params_.tf.publish_rate == 0.0 + ? std::nullopt + : std::optional{rclcpp::Duration::from_seconds(1.0 / params_.tf.publish_rate)}; + + try + { + pose_publisher_ = get_node()->create_publisher( + DEFAULT_POSE_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = + std::make_unique>( + pose_publisher_); + + if (params_.tf.enable) + { + tf_publisher_ = get_node()->create_publisher( + DEFAULT_TF_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_tf_publisher_ = + std::make_unique>( + tf_publisher_); + } + } + catch (const std::exception & ex) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message: %s\n", + ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // Initialize pose message + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->unlock(); + + // Initialize tf message if tf publishing is enabled + if (realtime_tf_publisher_) + { + realtime_tf_publisher_->lock(); + + realtime_tf_publisher_->msg_.transforms.resize(1); + auto & tf_transform = realtime_tf_publisher_->msg_.transforms.front(); + tf_transform.header.frame_id = params_.frame_id; + if (params_.tf.child_frame_id.empty()) + { + tf_transform.child_frame_id = params_.pose_name; + } + else + { + tf_transform.child_frame_id = params_.tf.child_frame_id; + } + + realtime_tf_publisher_->unlock(); + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->assign_loaned_state_interfaces(state_interfaces_); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + pose_sensor_->release_interfaces(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PoseBroadcaster::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + geometry_msgs::msg::Pose pose; + pose_sensor_->get_values_as_message(pose); + + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + realtime_publisher_->msg_.pose = pose; + realtime_publisher_->unlockAndPublish(); + } + + if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + { + bool do_publish = false; + // rlcpp::Time comparisons throw if clock types are not the same + if (tf_last_publish_time_.get_clock_type() != time.get_clock_type()) + { + do_publish = true; + } + else if (!tf_publish_period_ || (tf_last_publish_time_ + *tf_publish_period_ <= time)) + { + do_publish = true; + } + + if (do_publish) + { + auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + tf_transform.header.stamp = time; + + tf_transform.transform.translation.x = pose.position.x; + tf_transform.transform.translation.y = pose.position.y; + tf_transform.transform.translation.z = pose.position.z; + + tf_transform.transform.rotation.x = pose.orientation.x; + tf_transform.transform.rotation.y = pose.orientation.y; + tf_transform.transform.rotation.z = pose.orientation.z; + tf_transform.transform.rotation.w = pose.orientation.w; + + realtime_tf_publisher_->unlockAndPublish(); + + tf_last_publish_time_ = time; + } + else + { + realtime_tf_publisher_->unlock(); + } + } + + return controller_interface::return_type::OK; +} + +} // namespace pose_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(pose_broadcaster::PoseBroadcaster, controller_interface::ControllerInterface) diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml new file mode 100644 index 0000000000..11c53b5e57 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -0,0 +1,32 @@ +pose_broadcaster: + frame_id: + type: string + default_value: "" + description: "frame_id in which values are published" + validation: + not_empty<>: null + pose_name: + type: string + default_value: "" + description: "Base name used as prefix for controller interfaces. + The state interface names are: ``/position.x, ..., /position.z, + /orientation.x, ..., /orientation.w``" + validation: + not_empty<>: null + tf: + enable: + type: bool + default_value: true + description: "Whether to publish the pose as a tf transform" + child_frame_id: + type: string + default_value: "" + description: "Child frame id of published tf transforms. Defaults to ``pose_name`` if left + empty." + publish_rate: + type: double + default_value: 0.0 + description: "Rate to limit publishing of tf transforms to (Hz). If set to 0, no limiting is + performed." + validation: + gt_eq<>: 0.0 diff --git a/pose_broadcaster/test/pose_broadcaster_params.yaml b/pose_broadcaster/test/pose_broadcaster_params.yaml new file mode 100644 index 0000000000..a2f8477dd1 --- /dev/null +++ b/pose_broadcaster/test/pose_broadcaster_params.yaml @@ -0,0 +1,4 @@ +test_pose_broadcaster: + ros__parameters: + pose_name: "test_pose" + frame_id: "pose_frame" diff --git a/pose_broadcaster/test/test_load_pose_broadcaster.cpp b/pose_broadcaster/test/test_load_pose_broadcaster.cpp new file mode 100644 index 0000000000..bdf72d7b23 --- /dev/null +++ b/pose_broadcaster/test/test_load_pose_broadcaster.cpp @@ -0,0 +1,50 @@ +// Copyright 2024 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. +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadPoseBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm{ + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"}; + + const std::string test_file_path = + std::string{TEST_FILES_DIRECTORY} + "/pose_broadcaster_params.yaml"; + cm.set_parameter({"test_pose_broadcaster.params_file", test_file_path}); + + cm.set_parameter({"test_pose_broadcaster.type", "pose_broadcaster/PoseBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_pose_broadcaster"), nullptr); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp new file mode 100644 index 0000000000..0ed2e84619 --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -0,0 +1,198 @@ +// Copyright 2024 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. +#include "test_pose_broadcaster.hpp" + +#include +#include + +using hardware_interface::LoanedStateInterface; + +void PoseBroadcasterTest::SetUp() { pose_broadcaster_ = std::make_unique(); } + +void PoseBroadcasterTest::TearDown() { pose_broadcaster_.reset(nullptr); } + +void PoseBroadcasterTest::SetUpPoseBroadcaster() +{ + ASSERT_EQ( + pose_broadcaster_->init( + "test_pose_broadcaster", "", 0, "", pose_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector state_interfaces; + state_interfaces.emplace_back(pose_position_x_); + state_interfaces.emplace_back(pose_position_y_); + state_interfaces.emplace_back(pose_position_z_); + state_interfaces.emplace_back(pose_orientation_x_); + state_interfaces.emplace_back(pose_orientation_y_); + state_interfaces.emplace_back(pose_orientation_z_); + state_interfaces.emplace_back(pose_orientation_w_); + + pose_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); +} + +TEST_F(PoseBroadcasterTest, Configure_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command interface configuration + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ(command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + // Verify state interface configuration + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); +} + +TEST_F(PoseBroadcasterTest, Activate_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); + } + + // Deactivate controller + ASSERT_EQ( + pose_broadcaster_->on_deactivate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); // Should not change when deactivating + } +} + +TEST_F(PoseBroadcasterTest, Update_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_EQ( + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PoseBroadcasterTest, PublishSuccess) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_EQ(pose_msg.pose.position.x, pose_values_[0]); + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + subscribe_and_get_message("/tf", tf_msg); + + // Verify content of tf message + ASSERT_EQ(tf_msg.transforms.size(), 1lu); + EXPECT_EQ(tf_msg.transforms[0].header.frame_id, frame_id_); + EXPECT_EQ(tf_msg.transforms[0].child_frame_id, tf_child_frame_id_); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.x, pose_values_[0]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.y, pose_values_[1]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.z, pose_values_[2]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.x, pose_values_[3]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.y, pose_values_[4]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.z, pose_values_[5]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.w, pose_values_[6]); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp new file mode 100644 index 0000000000..a164b2c6ac --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -0,0 +1,96 @@ +// Copyright 2024 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. +#ifndef TEST_POSE_BROADCASTER_HPP_ +#define TEST_POSE_BROADCASTER_HPP_ + +#include + +#include +#include +#include + +#include "rclcpp/executors.hpp" + +#include "pose_broadcaster/pose_broadcaster.hpp" + +using pose_broadcaster::PoseBroadcaster; + +class PoseBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + + void SetUpPoseBroadcaster(); + +protected: + const std::string pose_name_ = "test_pose"; + const std::string frame_id_ = "pose_base_frame"; + const std::string tf_child_frame_id_ = "pose_frame"; + + std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; + + hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; + hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; + hardware_interface::StateInterface pose_position_z_{pose_name_, "position.x", &pose_values_[2]}; + hardware_interface::StateInterface pose_orientation_x_{ + pose_name_, "orientation.x", &pose_values_[3]}; + hardware_interface::StateInterface pose_orientation_y_{ + pose_name_, "orientation.y", &pose_values_[4]}; + hardware_interface::StateInterface pose_orientation_z_{ + pose_name_, "orientation.z", &pose_values_[5]}; + hardware_interface::StateInterface pose_orientation_w_{ + pose_name_, "orientation.w", &pose_values_[6]}; + + std::unique_ptr pose_broadcaster_; + + template + void subscribe_and_get_message(const std::string & topic, T & msg); +}; + +template +void PoseBroadcasterTest::subscribe_and_get_message(const std::string & topic, T & msg) +{ + // Create node for subscribing + rclcpp::Node node{"test_subscription_node"}; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node.get_node_base_interface()); + + // Create subscription + typename T::SharedPtr received_msg; + const auto msg_callback = [&](const typename T::SharedPtr sub_msg) { received_msg = sub_msg; }; + const auto subscription = node.create_subscription(topic, 10, msg_callback); + + // Update controller and spin until a message is received + // Since update doesn't guarantee a published message, republish until received + constexpr size_t max_sub_check_loop_count = 5; + for (size_t i = 0; !received_msg; ++i) + { + ASSERT_LT(i, max_sub_check_loop_count); + + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); + + const auto timeout = std::chrono::milliseconds{5}; + const auto until = node.get_clock()->now() + timeout; + while (!received_msg && node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds{10}); + } + } + + msg = *received_msg; +} + +#endif // TEST_POSE_BROADCASTER_HPP_ diff --git a/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 1aa9b26515..d52201f2b4 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- diff --git a/position_controllers/package.xml b/position_controllers/package.xml index 9545700d07..c03888eaf8 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.13.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/range_sensor_broadcaster/CHANGELOG.rst b/range_sensor_broadcaster/CHANGELOG.rst index 5a60a36dcd..77eae62d2a 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index cb814b1b8f..92283fe3ad 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.13.0 + 4.15.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index cb8ac7281f..bd9e0a8b38 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index 5a5ad3f632..0886b489c7 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.13.0 + 4.15.0 Metapackage for ROS2 controllers related packages Bence Magyar Jordan Palacios diff --git a/ros2_controllers_test_nodes/CHANGELOG.rst b/ros2_controllers_test_nodes/CHANGELOG.rst index c4e08ae2c1..29ca6cd84d 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* Fix deprecation warning in paramater declaration (`#1280 `_) +* Contributors: Sanjeev + 4.13.0 (2024-08-22) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index d1fd4d3844..f61a51e34e 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.13.0 + 4.15.0 Demo nodes for showing and testing functionalities of the ros2_control framework. Denis Štogl diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py index 5cf28ac604..bb6add77ef 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_forward_position_controller.py @@ -37,7 +37,7 @@ def __init__(self): # Read all positions from parameters self.goals = [] for name in goal_names: - self.declare_parameter(name) + self.declare_parameter(name, rclpy.Parameter.Type.DOUBLE_ARRAY) goal = self.get_parameter(name).value if goal is None or len(goal) == 0: raise Exception(f'Values for goal "{name}" not set!') diff --git a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py index cb66f58468..27f28da1be 100644 --- a/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py +++ b/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py @@ -18,7 +18,6 @@ import rclpy from rclpy.node import Node from builtin_interfaces.msg import Duration -from rcl_interfaces.msg import ParameterDescriptor from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sensor_msgs.msg import JointState @@ -67,8 +66,7 @@ def __init__(self): # Read all positions from parameters self.goals = [] # List of JointTrajectoryPoint for name in goal_names: - self.declare_parameter(name, descriptor=ParameterDescriptor(dynamic_typing=True)) - + self.declare_parameter(name, rclpy.Parameter.Type.DOUBLE_ARRAY) point = JointTrajectoryPoint() def get_sub_param(sub_param): diff --git a/ros2_controllers_test_nodes/setup.py b/ros2_controllers_test_nodes/setup.py index a9c8fb6b60..59aee66875 100644 --- a/ros2_controllers_test_nodes/setup.py +++ b/ros2_controllers_test_nodes/setup.py @@ -20,7 +20,7 @@ setup( name=package_name, - version="4.13.0", + version="4.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/rqt_joint_trajectory_controller/CHANGELOG.rst b/rqt_joint_trajectory_controller/CHANGELOG.rst index f3371a8320..043930c536 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* Fix bug for displaying all controllers (`#1259 `_) +* Contributors: Francisco Martín Rico + 4.13.0 (2024-08-22) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index c9b086e587..7920e63246 100644 --- a/rqt_joint_trajectory_controller/package.xml +++ b/rqt_joint_trajectory_controller/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> rqt_joint_trajectory_controller - 4.13.0 + 4.15.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index cc2fc8b00b..a70342e154 100644 --- a/rqt_joint_trajectory_controller/setup.py +++ b/rqt_joint_trajectory_controller/setup.py @@ -21,7 +21,7 @@ setup( name=package_name, - version="4.13.0", + version="4.15.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/steering_controllers_library/CHANGELOG.rst b/steering_controllers_library/CHANGELOG.rst index 8b2791a8e7..39360d6921 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* fix(timeout): do not reset steer wheels to 0. on timeout (`#1289 `_) +* fix(steering-odometry): convert twist to steering angle (`#1288 `_) +* Contributors: Manuel Muth, Rein Appeldoorn + +4.14.0 (2024-09-11) +------------------- +* fix(steering-odometry): handle infinite turning radius properly (`#1285 `_) +* Contributors: Rein Appeldoorn + 4.13.0 (2024-08-22) ------------------- diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index bff5ccc8e5..5c96c4624c 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.13.0 + 4.15.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f193098f82..836574f150 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -355,26 +355,11 @@ controller_interface::return_type SteeringControllersLibrary::update_reference_f const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { auto current_ref = *(input_ref_.readFromRT()); - const auto age_of_last_command = time - (current_ref)->header.stamp; - // send message only if there is no timeout - if (age_of_last_command <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) + if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.angular.z; - } - } - else - { - if (!std::isnan(current_ref->twist.linear.x) && !std::isnan(current_ref->twist.angular.z)) - { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - current_ref->twist.linear.x = std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = std::numeric_limits::quiet_NaN(); - } + reference_interfaces_[0] = current_ref->twist.linear.x; + reference_interfaces_[1] = current_ref->twist.angular.z; } return controller_interface::return_type::OK; @@ -396,13 +381,17 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c last_linear_velocity_ = reference_interfaces_[0]; last_angular_velocity_ = reference_interfaces_[1]; + const auto age_of_last_command = time - (*(input_ref_.readFromRT()))->header.stamp; + const auto timeout = + age_of_last_command > ref_timeout_ && ref_timeout_ != rclcpp::Duration::from_seconds(0); + auto [traction_commands, steering_commands] = odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); if (params_.front_steering) { for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { @@ -414,7 +403,7 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c { for (size_t i = 0; i < params_.front_wheels_names.size(); i++) { - command_interfaces_[i].set_value(traction_commands[i]); + command_interfaces_[i].set_value(timeout ? 0. : traction_commands[i]); } for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) { diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index ba431faf33..dbe210ed41 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -133,11 +133,17 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( const double steer_pos) { double turning_radius = wheelbase_ / std::tan(steer_pos); + const double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; + const double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; + + if (std::isinf(turning_radius)) + { + return (vel_wheel_r + vel_wheel_l) * 0.5; + } + // overdetermined, we take the average - double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius / - (turning_radius + wheel_track_ * 0.5); - double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius / - (turning_radius - wheel_track_ * 0.5); + const double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); + const double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); return (vel_r + vel_l) * 0.5; } @@ -202,12 +208,9 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { - if (fabs(v_bx) < std::numeric_limits::epsilon()) - { - // avoid division by zero - return 0.; - } - return std::atan(omega_bz * wheelbase_ / v_bx); + // phi can be nan if both v_bx and omega_bz are zero + const auto phi = std::atan(omega_bz * wheelbase_ / v_bx); + return std::isfinite(phi) ? phi : 0.0; } std::tuple, std::vector> SteeringOdometry::get_commands( diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index fca7d00946..3378efbef8 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -70,9 +70,9 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); - EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]); } } @@ -133,8 +133,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -142,10 +142,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); // case 2 position_feedback = true controller_->params_.position_feedback = true; @@ -174,8 +177,8 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) { EXPECT_TRUE(std::isnan(interface)); } - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x)); - EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z)); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.linear.x, TEST_LINEAR_VELOCITY_X); + ASSERT_EQ((*(controller_->input_ref_.readFromNonRT()))->twist.angular.z, TEST_ANGULAR_VELOCITY_Z); EXPECT_TRUE(std::isnan(controller_->reference_interfaces_[0])); for (const auto & interface : controller_->reference_interfaces_) @@ -183,10 +186,13 @@ TEST_F(SteeringControllersLibraryTest, test_both_update_methods_for_ref_timeout) EXPECT_TRUE(std::isnan(interface)); } - for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i) - { - EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0); - } + // Wheel velocities should reset to 0 + EXPECT_EQ(controller_->command_interfaces_[0].get_value(), 0); + EXPECT_EQ(controller_->command_interfaces_[1].get_value(), 0); + + // Steer angles should not reset + EXPECT_NEAR(controller_->command_interfaces_[2].get_value(), 0.575875, 1e-6); + EXPECT_NEAR(controller_->command_interfaces_[3].get_value(), 0.575875, 1e-6); } int main(int argc, char ** argv) diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index a4c6912ce0..34520eeb83 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- +* rename get/set_state to get/set_lifecylce_state (`#1250 `_) +* Contributors: Manuel Muth + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index aa37106fac..4e6ab510f3 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.13.0 + 4.15.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index f535a1309e..4e1f9abd1e 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- +* Adapt test to new way of exporting reference interfaces (Related to `#1240 `_ in ros2_control) (`#1103 `_) +* Contributors: Manuel Muth + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- * Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 041f4425e8..27aac82dd1 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.13.0 + 4.15.0 Steering controller with tricycle kinematics. Rear fixed wheels are powering the vehicle and front wheel is steering. Apache License 2.0 Bence Magyar diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 3f2589cb6c..8e29314f8e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -81,9 +81,9 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 2170659ee7..566169e34e 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -84,9 +84,9 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) { const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; - EXPECT_EQ(ref_if_conf[i].get_name(), ref_itf_name); - EXPECT_EQ(ref_if_conf[i].get_prefix_name(), controller_->get_node()->get_name()); - EXPECT_EQ(ref_if_conf[i].get_interface_name(), joint_reference_interfaces_[i]); + EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name); + EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]); } } diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 7b0e141de7..a04bf1c4ae 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.15.0 (2024-10-07) +------------------- + +4.14.0 (2024-09-11) +------------------- + 4.13.0 (2024-08-22) ------------------- diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index f7a6fcac72..ea295db0ed 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.13.0 + 4.15.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios