diff --git a/.github/workflows/humble-abi-compatibility.yml b/.github/workflows/humble-abi-compatibility.yml index d72d3725b3..247371ba7d 100644 --- a/.github/workflows/humble-abi-compatibility.yml +++ b/.github/workflows/humble-abi-compatibility.yml @@ -8,8 +8,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-abi-compatibility.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-abi-compatibility.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' diff --git a/.github/workflows/humble-binary-build.yml b/.github/workflows/humble-binary-build.yml index a78f6135b6..a031d823cf 100644 --- a/.github/workflows/humble-binary-build.yml +++ b/.github/workflows/humble-binary-build.yml @@ -11,8 +11,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' @@ -23,8 +24,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.humble.repos' diff --git a/.github/workflows/humble-coverage-build.yml b/.github/workflows/humble-coverage-build.yml index f39758a512..d0dcb9c350 100644 --- a/.github/workflows/humble-coverage-build.yml +++ b/.github/workflows/humble-coverage-build.yml @@ -8,9 +8,10 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' + - '**.yaml' - '.github/workflows/humble-coverage-build.yml' - 'codecov.yml' - - '**.yaml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' @@ -21,9 +22,10 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' + - '**.yaml' - '.github/workflows/humble-coverage-build.yml' - 'codecov.yml' - - '**.yaml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml index b018fcea84..cd25248caf 100644 --- a/.github/workflows/humble-debian-build.yml +++ b/.github/workflows/humble-debian-build.yml @@ -8,8 +8,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-debian-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-debian-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' diff --git a/.github/workflows/humble-rhel-semi-binary-build.yml b/.github/workflows/humble-rhel-semi-binary-build.yml index 2a7814f8b3..25c755ada5 100644 --- a/.github/workflows/humble-rhel-semi-binary-build.yml +++ b/.github/workflows/humble-rhel-semi-binary-build.yml @@ -8,8 +8,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-rhel-semi-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-rhel-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' diff --git a/.github/workflows/humble-semi-binary-build.yml b/.github/workflows/humble-semi-binary-build.yml index 72287fa115..4246d18160 100644 --- a/.github/workflows/humble-semi-binary-build.yml +++ b/.github/workflows/humble-semi-binary-build.yml @@ -10,8 +10,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-semi-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' @@ -22,8 +23,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/humble-semi-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/humble-semi-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers.humble.repos' diff --git a/.github/workflows/humble-source-build.yml b/.github/workflows/humble-source-build.yml index 44d474ded4..94af6edd8c 100644 --- a/.github/workflows/humble-source-build.yml +++ b/.github/workflows/humble-source-build.yml @@ -8,6 +8,8 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' + - '**.yaml' - '.github/workflows/humble-source-build.yml' - '**/package.xml' - '**/CMakeLists.txt' diff --git a/.github/workflows/iron-binary-build.yml b/.github/workflows/iron-binary-build.yml index 802ca97bb8..0d2f0ff7f3 100644 --- a/.github/workflows/iron-binary-build.yml +++ b/.github/workflows/iron-binary-build.yml @@ -13,8 +13,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/iron-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/iron-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.iron.repos' @@ -25,8 +26,9 @@ on: - '**.hpp' - '**.h' - '**.cpp' - - '.github/workflows/iron-binary-build.yml' + - '**.py' - '**.yaml' + - '.github/workflows/iron-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' - 'ros2_controllers-not-released.iron.repos' diff --git a/.github/workflows/iron-coverage-build.yml b/.github/workflows/iron-coverage-build.yml index bbc9c4e975..e1d2e84bc8 100644 --- a/.github/workflows/iron-coverage-build.yml +++ b/.github/workflows/iron-coverage-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-coverage-build.yml' - 'codecov.yml' @@ -21,6 +22,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-coverage-build.yml' - 'codecov.yml' diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml index 105eb29aba..ebd2f46c18 100644 --- a/.github/workflows/iron-debian-build.yml +++ b/.github/workflows/iron-debian-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-debian-build.yml' - '**/package.xml' diff --git a/.github/workflows/iron-semi-binary-build.yml b/.github/workflows/iron-semi-binary-build.yml index bf5edcaa56..3aca5e5b70 100644 --- a/.github/workflows/iron-semi-binary-build.yml +++ b/.github/workflows/iron-semi-binary-build.yml @@ -12,6 +12,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/iron-source-build.yml b/.github/workflows/iron-source-build.yml index c460762259..c54ead7877 100644 --- a/.github/workflows/iron-source-build.yml +++ b/.github/workflows/iron-source-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/iron-source-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-binary-build.yml b/.github/workflows/jazzy-binary-build.yml index 6ec46eb6c1..cda4969abf 100644 --- a/.github/workflows/jazzy-binary-build.yml +++ b/.github/workflows/jazzy-binary-build.yml @@ -13,6 +13,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-debian-build.yml b/.github/workflows/jazzy-debian-build.yml index 61a47d0f2e..e3e3b8a353 100644 --- a/.github/workflows/jazzy-debian-build.yml +++ b/.github/workflows/jazzy-debian-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-debian-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-rhel-semi-binary-build.yml b/.github/workflows/jazzy-rhel-semi-binary-build.yml index 94ddfeb54f..1c62fcf2ac 100644 --- a/.github/workflows/jazzy-rhel-semi-binary-build.yml +++ b/.github/workflows/jazzy-rhel-semi-binary-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-rhel-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-semi-binary-build.yml b/.github/workflows/jazzy-semi-binary-build.yml index 7c5bdfc4dc..ffa4f914b9 100644 --- a/.github/workflows/jazzy-semi-binary-build.yml +++ b/.github/workflows/jazzy-semi-binary-build.yml @@ -12,6 +12,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-semi-binary-build.yml' - '**/package.xml' @@ -24,6 +25,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/jazzy-source-build.yml b/.github/workflows/jazzy-source-build.yml index f77e9a5060..45f6d1d25e 100644 --- a/.github/workflows/jazzy-source-build.yml +++ b/.github/workflows/jazzy-source-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/jazzy-source-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-binary-build.yml b/.github/workflows/rolling-binary-build.yml index 529c13ac64..b512eb9db7 100644 --- a/.github/workflows/rolling-binary-build.yml +++ b/.github/workflows/rolling-binary-build.yml @@ -13,6 +13,8 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' + - '**.yaml' - '.github/workflows/rolling-binary-build.yml' - '**/package.xml' - '**/CMakeLists.txt' @@ -24,6 +26,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-coverage-build.yml b/.github/workflows/rolling-coverage-build.yml index 3b4b195ff7..058ff9bb33 100644 --- a/.github/workflows/rolling-coverage-build.yml +++ b/.github/workflows/rolling-coverage-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-coverage-build.yml' - 'codecov.yml' @@ -21,6 +22,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-coverage-build.yml' - 'codecov.yml' diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml index c1313d7d5d..d7efd3ea0a 100644 --- a/.github/workflows/rolling-debian-build.yml +++ b/.github/workflows/rolling-debian-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-debian-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-rhel-semi-binary-build.yml b/.github/workflows/rolling-rhel-semi-binary-build.yml index 6b7fb6b8c4..29a53b810a 100644 --- a/.github/workflows/rolling-rhel-semi-binary-build.yml +++ b/.github/workflows/rolling-rhel-semi-binary-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-rhel-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-semi-binary-build.yml b/.github/workflows/rolling-semi-binary-build.yml index db0cd33de5..872c509931 100644 --- a/.github/workflows/rolling-semi-binary-build.yml +++ b/.github/workflows/rolling-semi-binary-build.yml @@ -12,6 +12,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-semi-binary-build.yml' - '**/package.xml' @@ -24,6 +25,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-semi-binary-build.yml' - '**/package.xml' diff --git a/.github/workflows/rolling-source-build.yml b/.github/workflows/rolling-source-build.yml index 66e53d1da1..e19a1e8cf7 100644 --- a/.github/workflows/rolling-source-build.yml +++ b/.github/workflows/rolling-source-build.yml @@ -8,6 +8,7 @@ on: - '**.hpp' - '**.h' - '**.cpp' + - '**.py' - '**.yaml' - '.github/workflows/rolling-source-build.yml' - '**/package.xml' diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6fadbbdace..43bb778260 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -50,13 +50,13 @@ 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.8.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"] @@ -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.2 hooks: - id: check-github-workflows args: ["--verbose"] diff --git a/ackermann_steering_controller/CHANGELOG.rst b/ackermann_steering_controller/CHANGELOG.rst index 8ce11f3a9e..066ef29830 100644 --- a/ackermann_steering_controller/CHANGELOG.rst +++ b/ackermann_steering_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package ackermann_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index a34be3c672..83edf7c157 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -58,10 +58,8 @@ if(BUILD_TESTING) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_ackermann_steering_controller - test/test_load_ackermann_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_ackermann_steering_controller test/test_load_ackermann_steering_controller.cpp) ament_target_dependencies(test_load_ackermann_steering_controller controller_manager hardware_interface diff --git a/ackermann_steering_controller/package.xml b/ackermann_steering_controller/package.xml index 62e9a5a27b..3c9836e97b 100644 --- a/ackermann_steering_controller/package.xml +++ b/ackermann_steering_controller/package.xml @@ -2,7 +2,7 @@ ackermann_steering_controller - 4.12.0 + 4.14.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_load_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp index 8e81643835..e7b56e2788 100644 --- a/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_load_ackermann_steering_controller.cpp @@ -29,12 +29,15 @@ TEST(TestLoadAckermannSteeringController, load_controller) 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) + "/ackermann_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_ackermann_steering_controller", - "ackermann_steering_controller/AckermannSteeringController"), - nullptr); + cm.set_parameter({"test_ackermann_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_ackermann_steering_controller.type", + "ackermann_steering_controller/AckermannSteeringController"}); + + ASSERT_NE(cm.load_controller("test_ackermann_steering_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/admittance_controller/CHANGELOG.rst b/admittance_controller/CHANGELOG.rst index 390eae4878..bb640c49fe 100644 --- a/admittance_controller/CHANGELOG.rst +++ b/admittance_controller/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package admittance_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fix segfault at reconfigure of AdmittanceController (`#1248 `_) +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Lennart Nachtigall, Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- +* Fix admittance controller interface read/write logic (`#1232 `_) +* Contributors: Nikola Banović + 4.12.0 (2024-07-23) ------------------- * Change the subscription timeout in the tests to 5ms (`#1219 `_) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 10ca5caa40..cd16714a31 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -68,10 +68,8 @@ if(BUILD_TESTING) find_package(kinematics_interface_kdl REQUIRED) # test loading admittance controller - add_rostest_with_parameters_gmock(test_load_admittance_controller - test/test_load_admittance_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/test_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_admittance_controller test/test_load_admittance_controller.cpp) ament_target_dependencies(test_load_admittance_controller controller_manager hardware_interface diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp index 9be6c3298c..9b2a22abf7 100644 --- a/admittance_controller/include/admittance_controller/admittance_controller.hpp +++ b/admittance_controller/include/admittance_controller/admittance_controller.hpp @@ -133,6 +133,8 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // ROS subscribers rclcpp::Subscription::SharedPtr input_joint_command_subscriber_; + rclcpp::Subscription::SharedPtr + input_goal_pose_subscriber_; rclcpp::Publisher::SharedPtr s_publisher_; // admittance parameters @@ -140,10 +142,13 @@ class AdmittanceController : public controller_interface::ChainableControllerInt // ROS messages std::shared_ptr joint_command_msg_; + std::shared_ptr goal_pose_msg_; // real-time buffer realtime_tools::RealtimeBuffer> input_joint_command_; + realtime_tools::RealtimeBuffer> + input_goal_pose_; std::unique_ptr> state_publisher_; trajectory_msgs::msg::JointTrajectoryPoint last_commanded_; diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 7223dbe9d1..cdc39af07e 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -29,6 +29,8 @@ #include "kinematics_interface/kinematics_interface.hpp" #include "pluginlib/class_loader.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" namespace admittance_controller { @@ -102,20 +104,38 @@ class AdmittanceRule /// Configure admittance rule memory using number of joints. controller_interface::return_type configure( - const std::shared_ptr & node, const size_t num_joint); + const std::shared_ptr & node, const size_t num_joint, + const std::string & robot_description); /// Reset all values back to default controller_interface::return_type reset(const size_t num_joints); /** - * Calculate all transforms needed for admittance control using the loader kinematics plugin. If + * Calculate transforms needed for admittance control using the loader kinematics plugin. If * the transform does not exist in the kinematics model, then TF will be used for lookup. The - * return value is true if all transformation are calculated without an error \param[in] - * current_joint_state current joint state of the robot \param[in] reference_joint_state input - * joint state reference \param[out] success true if no calls to the kinematics interface fail + * return value is true if all transformation are calculated without an error + * \param[in] current_joint_state current joint state of the robot + * \param[out] success true if no calls to the kinematics interface fail + */ + bool get_current_state_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state); + /** + * Calculate ft_sensor_link -> base_link transform needed for admittance control using the loader kinematics plugin. If + * the transform does not exist in the kinematics model, then TF will be used for lookup. The + * return value is true if all transformation are calculated without an error + * \param[in] reference_pose input ft sensor reference pose + * \param[out] success true if no calls to the kinematics interface fail */ - bool get_all_transforms( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + bool get_reference_state_transforms( + const geometry_msgs::msg::PoseStamped & reference_pose); + /** + * Calculate ft_sensor_link -> base_link transform needed for admittance control using the loader kinematics plugin. If + * the transform does not exist in the kinematics model, then TF will be used for lookup. The + * return value is true if all transformation are calculated without an error + * \param[in] reference_joint_state input reference joint state + * \param[out] success true if no calls to the kinematics interface fail + */ + bool get_reference_state_transforms( const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state); /** @@ -125,6 +145,24 @@ class AdmittanceRule */ void apply_parameters_update(); + /** + * Calculate 'desired joint states' based on the 'measured force', 'reference goal pose', and + * 'current_joint_state'. + * + * \param[in] current_joint_state current joint state of the robot + * \param[in] measured_wrench most recent measured wrench from force torque sensor + * \param[in] reference_pose input reference pose + * \param[in] period time in seconds since last controller update + * \param[out] desired_joint_state joint state reference after the admittance offset is applied to + * the input reference + */ + controller_interface::return_type update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const geometry_msgs::msg::PoseStamped & reference_pose, + const rclcpp::Duration & period, + trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states); + /** * Calculate 'desired joint states' based on the 'measured force', 'reference joint state', and * 'current_joint_state'. @@ -151,12 +189,29 @@ class AdmittanceRule */ const control_msgs::msg::AdmittanceControllerState & get_controller_state(); + /** + * Explanation - uses kinematics + * + * \param[in] state_message message + * \param[out] state_message message + */ + geometry_msgs::msg::Pose initialize_goal_pose(const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state); + public: // admittance config parameters std::shared_ptr parameter_handler_; admittance_controller::Params parameters_; protected: + /** + * Populate the AdmittanceState member using the current AdmittanceTransforms + * \param[in] current_joint_state + * \param[in] measured_wrench + */ + void get_admittance_state_from_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench); + /** * Calculates the admittance rule from given the robot's current joint angles. The admittance * controller state input is updated with the new calculated values. A boolean value is returned diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 77f1277b35..13af8fe600 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -20,6 +20,7 @@ #include "admittance_controller/admittance_rule.hpp" #include +#include #include #include @@ -34,7 +35,8 @@ constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation) /// Configure admittance rule memory for num joints and load kinematics interface controller_interface::return_type AdmittanceRule::configure( - const std::shared_ptr & node, const size_t num_joints) + const std::shared_ptr & node, const size_t num_joints, + const std::string & robot_description) { num_joints_ = num_joints; @@ -46,13 +48,21 @@ controller_interface::return_type AdmittanceRule::configure( { try { + // Make sure we destroy the interface first. Otherwise we might run into a segfault + if (kinematics_loader_) + { + kinematics_.reset(); + } kinematics_loader_ = std::make_shared>( parameters_.kinematics.plugin_package, "kinematics_interface::KinematicsInterface"); kinematics_ = std::unique_ptr( kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name)); + if (!kinematics_->initialize( - node->get_node_parameters_interface(), parameters_.kinematics.tip)) + robot_description, + node->get_node_parameters_interface(), + "kinematics")) { return controller_interface::return_type::ERROR; } @@ -113,6 +123,15 @@ controller_interface::return_type AdmittanceRule::reset(const size_t num_joints) return controller_interface::return_type::OK; } +geometry_msgs::msg::Pose AdmittanceRule::initialize_goal_pose( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) +{ + kinematics_->calculate_link_transform( + current_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); + return tf2::toMsg(admittance_transforms_.ref_base_ft_); +} + void AdmittanceRule::apply_parameters_update() { if (parameter_handler_->is_old(parameters_)) @@ -134,17 +153,12 @@ void AdmittanceRule::apply_parameters_update() } } -bool AdmittanceRule::get_all_transforms( - const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, - const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) -{ - // get reference transforms - bool success = kinematics_->calculate_link_transform( - reference_joint_state.positions, parameters_.ft_sensor.frame.id, - admittance_transforms_.ref_base_ft_); +bool AdmittanceRule::get_current_state_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state) +{ // get transforms at current configuration - success &= kinematics_->calculate_link_transform( + bool success = kinematics_->calculate_link_transform( current_joint_state.positions, parameters_.ft_sensor.frame.id, admittance_transforms_.base_ft_); success &= kinematics_->calculate_link_transform( current_joint_state.positions, parameters_.kinematics.tip, admittance_transforms_.base_tip_); @@ -160,6 +174,20 @@ bool AdmittanceRule::get_all_transforms( return success; } +bool AdmittanceRule::get_reference_state_transforms( + const geometry_msgs::msg::PoseStamped & reference_pose) +{ + tf2::fromMsg(reference_pose.pose, admittance_transforms_.ref_base_ft_); + return true; +} +bool AdmittanceRule::get_reference_state_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state) +{ + bool success = kinematics_->calculate_link_transform( + reference_joint_state.positions, parameters_.ft_sensor.frame.id, + admittance_transforms_.ref_base_ft_); + return success; +} // Update from reference joint states controller_interface::return_type AdmittanceRule::update( @@ -174,27 +202,9 @@ controller_interface::return_type AdmittanceRule::update( { apply_parameters_update(); } - - bool success = get_all_transforms(current_joint_state, reference_joint_state); - - // apply filter and update wrench_world_ vector - Eigen::Matrix rot_world_sensor = - admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); - Eigen::Matrix rot_world_cog = - admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation(); - process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); - - // transform wrench_world_ into base frame - admittance_state_.wrench_base.block<3, 1>(0, 0) = - admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0); - admittance_state_.wrench_base.block<3, 1>(3, 0) = - admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0); - - // Compute admittance control law - vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); - admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation(); - admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; - admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; + bool success = get_current_state_transforms(current_joint_state); + success &= get_reference_state_transforms(reference_joint_state); + get_admittance_state_from_transforms(current_joint_state, measured_wrench); success &= calculate_admittance_rule(admittance_state_, dt); // if a failure occurred during any kinematics interface calls, return an error and don't @@ -215,10 +225,69 @@ controller_interface::return_type AdmittanceRule::update( desired_joint_state.accelerations[i] = reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; } + return controller_interface::return_type::OK; +} + +controller_interface::return_type AdmittanceRule::update( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench, + const geometry_msgs::msg::PoseStamped & reference_pose, + const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state) +{ + const double dt = period.seconds(); + + if (parameters_.enable_parameter_update_without_reactivation) + { + apply_parameters_update(); + } + bool success = get_current_state_transforms(current_joint_state); + success &= get_reference_state_transforms(reference_pose); + get_admittance_state_from_transforms(current_joint_state, measured_wrench); + success &= calculate_admittance_rule(admittance_state_, dt); + + // if a failure occurred during any kinematics interface calls, return an error and + // remain at current state + if (!success) + { + desired_joint_state = current_joint_state; + return controller_interface::return_type::ERROR; + } + + // update joint desired joint state + for (size_t i = 0; i < num_joints_; ++i) + { + desired_joint_state.positions[i] = admittance_state_.joint_pos[i]; + desired_joint_state.velocities[i] = admittance_state_.joint_vel[i]; + desired_joint_state.accelerations[i] = admittance_state_.joint_acc[i]; + } return controller_interface::return_type::OK; } +void AdmittanceRule::get_admittance_state_from_transforms( + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state, + const geometry_msgs::msg::Wrench & measured_wrench) +{ + // apply filter and update wrench_world_ vector + Eigen::Matrix rot_world_sensor = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_ft_.rotation(); + Eigen::Matrix rot_world_cog = + admittance_transforms_.world_base_.rotation() * admittance_transforms_.base_cog_.rotation(); + process_wrench_measurements(measured_wrench, rot_world_sensor, rot_world_cog); + + // transform wrench_world_ into base frame + admittance_state_.wrench_base.block<3, 1>(0, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(0, 0); + admittance_state_.wrench_base.block<3, 1>(3, 0) = + admittance_transforms_.world_base_.rotation().transpose() * wrench_world_.block<3, 1>(3, 0); + + // Compute admittance control law + vec_to_eigen(current_joint_state.positions, admittance_state_.current_joint_pos); + admittance_state_.rot_base_control = admittance_transforms_.base_control_.rotation(); + admittance_state_.ref_trans_base_ft = admittance_transforms_.ref_base_ft_; + admittance_state_.ft_sensor_frame = parameters_.ft_sensor.frame.id; +} + bool AdmittanceRule::calculate_admittance_rule(AdmittanceState & admittance_state, double dt) { // Create stiffness matrix in base frame. The user-provided values of admittance_state.stiffness diff --git a/admittance_controller/package.xml b/admittance_controller/package.xml index 2e056c12a7..d9d6d24b7e 100644 --- a/admittance_controller/package.xml +++ b/admittance_controller/package.xml @@ -2,7 +2,7 @@ admittance_controller - 4.12.0 + 4.14.0 Implementation of admittance controllers for different input and output interface. Denis Štogl Bence Magyar diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index a4b56d739c..e11a376799 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -265,6 +265,14 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( input_joint_command_subscriber_ = get_node()->create_subscription( "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback); + + auto goal_pose_callback = + [this](const std::shared_ptr msg) + { input_goal_pose_.writeFromNonRT(msg); }; + input_goal_pose_subscriber_ = + get_node()->create_subscription( + "~/goal_pose", rclcpp::SystemDefaultsQoS(), goal_pose_callback); + s_publisher_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); state_publisher_ = @@ -280,7 +288,9 @@ controller_interface::CallbackReturn AdmittanceController::on_configure( semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name)); // configure admittance rule - if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR) + if ( + admittance_->configure(get_node(), num_joints_, this->get_robot_description()) == + controller_interface::return_type::ERROR) { return controller_interface::CallbackReturn::ERROR; } @@ -344,6 +354,12 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( return controller_interface::CallbackReturn::ERROR; } } + goal_pose_msg_ = std::make_shared(); + goal_pose_msg_->pose = admittance_->initialize_goal_pose(joint_state_); + if(!goal_pose_msg_){ + RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize goal_pose from current joint positions.\n"); + return controller_interface::CallbackReturn::ERROR; + } // Use current joint_state as a default reference last_reference_ = joint_state_; @@ -378,6 +394,14 @@ controller_interface::return_type AdmittanceController::update_reference_from_su } } + // after initializing goal_pose_msg_, update it from subscribers only + // if another message exists + if(*input_goal_pose_.readFromRT()) + { + goal_pose_msg_ = *input_goal_pose_.readFromRT(); + } + + return controller_interface::return_type::OK; } @@ -390,6 +414,12 @@ controller_interface::return_type AdmittanceController::update_and_write_command return controller_interface::return_type::ERROR; } + if (period.seconds() > 5.0) { + RCLCPP_WARN( + get_node()->get_logger(), "Large dt, skipping!"); + return controller_interface::return_type::OK; + } + // update input reference from chainable interfaces read_state_reference_interfaces(reference_); @@ -397,7 +427,18 @@ controller_interface::return_type AdmittanceController::update_and_write_command read_state_from_hardware(joint_state_, ft_values_); // apply admittance control to reference to determine desired state - admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + + /* some flag that determines whether we call the update() with goal_pose or with joint reference. + How can we know here whether the reference was updated from subscribers or from itnerfaces? + + if(updated_from_subscribers) + admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, period, reference_admittance_); + else + admittance_->update(joint_state_, ft_values_, reference_, period, reference_admittance_); + + */ + + admittance_->update(joint_state_, ft_values_, *goal_pose_msg_, period, reference_admittance_); // write calculated values to joint interfaces write_state_to_hardware(reference_admittance_); diff --git a/admittance_controller/test/test_load_admittance_controller.cpp b/admittance_controller/test/test_load_admittance_controller.cpp index ea4c14caac..5006920858 100644 --- a/admittance_controller/test/test_load_admittance_controller.cpp +++ b/admittance_controller/test/test_load_admittance_controller.cpp @@ -30,10 +30,13 @@ TEST(TestLoadAdmittanceController, load_controller) 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) + "/test_params.yaml"; - ASSERT_EQ( - cm.load_controller("load_admittance_controller", "admittance_controller/AdmittanceController"), - nullptr); + cm.set_parameter({"load_admittance_controller.params_file", test_file_path}); + cm.set_parameter( + {"load_admittance_controller.type", "admittance_controller/AdmittanceController"}); + + ASSERT_EQ(cm.load_controller("load_admittance_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/bicycle_steering_controller/CHANGELOG.rst b/bicycle_steering_controller/CHANGELOG.rst index ee689b77ae..d2a06f3329 100644 --- a/bicycle_steering_controller/CHANGELOG.rst +++ b/bicycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package bicycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/bicycle_steering_controller/CMakeLists.txt b/bicycle_steering_controller/CMakeLists.txt index 6535a73260..db1f5cf4ba 100644 --- a/bicycle_steering_controller/CMakeLists.txt +++ b/bicycle_steering_controller/CMakeLists.txt @@ -57,10 +57,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_bicycle_steering_controller - test/test_load_bicycle_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/bicycle_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_bicycle_steering_controller test/test_load_bicycle_steering_controller.cpp) ament_target_dependencies(test_load_bicycle_steering_controller controller_manager hardware_interface diff --git a/bicycle_steering_controller/package.xml b/bicycle_steering_controller/package.xml index 37a552cd7d..5858b53dc3 100644 --- a/bicycle_steering_controller/package.xml +++ b/bicycle_steering_controller/package.xml @@ -2,7 +2,7 @@ bicycle_steering_controller - 4.12.0 + 4.14.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_load_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp index 7be5e2dd1d..4d17a4bdd7 100644 --- a/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_load_bicycle_steering_controller.cpp @@ -29,11 +29,15 @@ TEST(TestLoadBicycleSteeringController, load_controller) 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) + "/bicycle_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_bicycle_steering_controller", "bicycle_steering_controller/BicycleSteeringController"), - nullptr); + cm.set_parameter({"test_bicycle_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_bicycle_steering_controller.type", + "bicycle_steering_controller/BicycleSteeringController"}); + + ASSERT_NE(cm.load_controller("test_bicycle_steering_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/diff_drive_controller/CHANGELOG.rst b/diff_drive_controller/CHANGELOG.rst index 6fcf3e272c..4907b259bf 100644 --- a/diff_drive_controller/CHANGELOG.rst +++ b/diff_drive_controller/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package diff_drive_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 70a54e302c..4eeb98b9d2 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -70,10 +70,8 @@ if(BUILD_TESTING) tf2_msgs ) - add_rostest_with_parameters_gmock(test_load_diff_drive_controller - test/test_load_diff_drive_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_diff_drive_controller test/test_load_diff_drive_controller.cpp) ament_target_dependencies(test_load_diff_drive_controller controller_manager ros2_control_test_assets diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index a819df01e0..7f4cb4cbd6 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -1,7 +1,7 @@ diff_drive_controller - 4.12.0 + 4.14.0 Controller for a differential drive mobile base. Bence Magyar Jordan Palacios diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a64f210503..66da6d6738 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -101,7 +101,7 @@ controller_interface::return_type DiffDriveController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { auto logger = get_node()->get_logger(); - if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { diff --git a/diff_drive_controller/test/test_load_diff_drive_controller.cpp b/diff_drive_controller/test/test_load_diff_drive_controller.cpp index aa23f83ec9..302bdf6924 100644 --- a/diff_drive_controller/test/test_load_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_load_diff_drive_controller.cpp @@ -29,10 +29,14 @@ TEST(TestLoadDiffDriveController, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::diffbot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/config/test_diff_drive_controller.yaml"; - ASSERT_NE( - cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"), - nullptr); + cm.set_parameter({"test_diff_drive_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_diff_drive_controller.type", "diff_drive_controller/DiffDriveController"}); + + ASSERT_NE(cm.load_controller("test_diff_drive_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/doc/writing_new_controller.rst b/doc/writing_new_controller.rst index 1a9ffce714..5aa7068b98 100644 --- a/doc/writing_new_controller.rst +++ b/doc/writing_new_controller.rst @@ -42,7 +42,7 @@ The following is a step-by-step guide to create source files, basic tests, and c 5. Add a constructor without parameters and the following public methods overriding the ``ControllerInterface`` definition: ``on_init``, ``command_interface_configuration``, ``state_interface_configuration``, ``on_configure``, ``on_activate``, ``on_deactivate``, ``update``. For exact definitions check the ``controller_interface/controller_interface.hpp`` header or one of the controllers from `ros2_controllers `_. - 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``get_node_options``. + 6. (Optional) The NodeOptions of the LifecycleNode can be personalized by overriding the default method ``define_custom_node_options``. 7. (Optional) Often, controllers accept lists of joint names and interface names as parameters. If so, you can add two protected string vectors to store those values. diff --git a/effort_controllers/CHANGELOG.rst b/effort_controllers/CHANGELOG.rst index 0f4271ba91..388fde8829 100644 --- a/effort_controllers/CHANGELOG.rst +++ b/effort_controllers/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package effort_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/effort_controllers/package.xml b/effort_controllers/package.xml index 6f3438c70e..1208cd4732 100644 --- a/effort_controllers/package.xml +++ b/effort_controllers/package.xml @@ -1,7 +1,7 @@ effort_controllers - 4.12.0 + 4.14.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 134faec535..30c2e36143 100644 --- a/force_torque_sensor_broadcaster/CHANGELOG.rst +++ b/force_torque_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package force_torque_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/force_torque_sensor_broadcaster/CMakeLists.txt b/force_torque_sensor_broadcaster/CMakeLists.txt index 38c984192e..02c343f050 100644 --- a/force_torque_sensor_broadcaster/CMakeLists.txt +++ b/force_torque_sensor_broadcaster/CMakeLists.txt @@ -53,9 +53,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_force_torque_sensor_broadcaster - test/test_load_force_torque_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/force_torque_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_force_torque_sensor_broadcaster test/test_load_force_torque_sensor_broadcaster.cpp) target_include_directories(test_load_force_torque_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_force_torque_sensor_broadcaster force_torque_sensor_broadcaster diff --git a/force_torque_sensor_broadcaster/package.xml b/force_torque_sensor_broadcaster/package.xml index 063239659b..758f448fdf 100644 --- a/force_torque_sensor_broadcaster/package.xml +++ b/force_torque_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ force_torque_sensor_broadcaster - 4.12.0 + 4.14.0 Controller to publish state of force-torque sensors. Bence Magyar Denis Štogl diff --git a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp index f8b367d341..b5887f09e1 100644 --- a/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp +++ b/force_torque_sensor_broadcaster/test/test_load_force_torque_sensor_broadcaster.cpp @@ -34,11 +34,15 @@ TEST(TestLoadForceTorqueSensorBroadcaster, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); - ASSERT_NE( - cm.load_controller( - "test_force_torque_sensor_broadcaster", - "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"), - nullptr); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/force_torque_sensor_broadcaster_params.yaml"; + + cm.set_parameter({"test_force_torque_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_force_torque_sensor_broadcaster.type", + "force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_force_torque_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/forward_command_controller/CHANGELOG.rst b/forward_command_controller/CHANGELOG.rst index bfabe442ac..04a2dd86c5 100644 --- a/forward_command_controller/CHANGELOG.rst +++ b/forward_command_controller/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package forward_command_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/forward_command_controller/package.xml b/forward_command_controller/package.xml index 7ff4e85139..bfa8a68955 100644 --- a/forward_command_controller/package.xml +++ b/forward_command_controller/package.xml @@ -1,7 +1,7 @@ forward_command_controller - 4.12.0 + 4.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios diff --git a/gripper_controllers/CHANGELOG.rst b/gripper_controllers/CHANGELOG.rst index 3334033fa9..fc2e5c571a 100644 --- a/gripper_controllers/CHANGELOG.rst +++ b/gripper_controllers/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package gripper_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/gripper_controllers/package.xml b/gripper_controllers/package.xml index a61a650c4d..019f1e2e44 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.12.0 + 4.14.0 The gripper_controllers package Bence Magyar diff --git a/imu_sensor_broadcaster/CHANGELOG.rst b/imu_sensor_broadcaster/CHANGELOG.rst index d73046d7e3..89a39358ed 100644 --- a/imu_sensor_broadcaster/CHANGELOG.rst +++ b/imu_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package imu_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/imu_sensor_broadcaster/CMakeLists.txt b/imu_sensor_broadcaster/CMakeLists.txt index 6782012c65..5539dea9ff 100644 --- a/imu_sensor_broadcaster/CMakeLists.txt +++ b/imu_sensor_broadcaster/CMakeLists.txt @@ -53,9 +53,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_imu_sensor_broadcaster - test/test_load_imu_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/imu_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_imu_sensor_broadcaster test/test_load_imu_sensor_broadcaster.cpp) target_include_directories(test_load_imu_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_imu_sensor_broadcaster imu_sensor_broadcaster diff --git a/imu_sensor_broadcaster/package.xml b/imu_sensor_broadcaster/package.xml index 5c3d3b1196..0ed268d9ad 100644 --- a/imu_sensor_broadcaster/package.xml +++ b/imu_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ imu_sensor_broadcaster - 4.12.0 + 4.14.0 Controller to publish readings of IMU sensors. Bence Magyar Denis Štogl diff --git a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp index fa2afab200..eca29082c1 100644 --- a/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp +++ b/imu_sensor_broadcaster/test/test_load_imu_sensor_broadcaster.cpp @@ -33,11 +33,14 @@ TEST(TestLoadIMUSensorBroadcaster, load_controller) 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) + "/imu_sensor_broadcaster_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster"), - nullptr); + cm.set_parameter({"test_imu_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_imu_sensor_broadcaster.type", "imu_sensor_broadcaster/IMUSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_imu_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/joint_state_broadcaster/CHANGELOG.rst b/joint_state_broadcaster/CHANGELOG.rst index 8ec080072f..21dfc85d82 100644 --- a/joint_state_broadcaster/CHANGELOG.rst +++ b/joint_state_broadcaster/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package joint_state_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/joint_state_broadcaster/CMakeLists.txt b/joint_state_broadcaster/CMakeLists.txt index 5c383897cc..cc8dc18bf6 100644 --- a/joint_state_broadcaster/CMakeLists.txt +++ b/joint_state_broadcaster/CMakeLists.txt @@ -16,6 +16,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils realtime_tools sensor_msgs + urdf ) find_package(ament_cmake REQUIRED) @@ -72,6 +73,7 @@ if(BUILD_TESTING) ) ament_target_dependencies(test_joint_state_broadcaster hardware_interface + ros2_control_test_assets ) endif() diff --git a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp index b3fa69f94c..ecc3c767f6 100644 --- a/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp @@ -27,6 +27,7 @@ #include "joint_state_broadcaster_parameters.hpp" #include "realtime_tools/realtime_publisher.h" #include "sensor_msgs/msg/joint_state.hpp" +#include "urdf/model.h" namespace joint_state_broadcaster { @@ -111,6 +112,9 @@ class JointStateBroadcaster : public controller_interface::ControllerInterface dynamic_joint_state_publisher_; std::shared_ptr> realtime_dynamic_joint_state_publisher_; + + urdf::Model model_; + bool is_model_loaded_ = false; }; } // namespace joint_state_broadcaster diff --git a/joint_state_broadcaster/package.xml b/joint_state_broadcaster/package.xml index a6bee4983a..8776cb31d3 100644 --- a/joint_state_broadcaster/package.xml +++ b/joint_state_broadcaster/package.xml @@ -1,7 +1,7 @@ joint_state_broadcaster - 4.12.0 + 4.14.0 Broadcaster to publish joint state Bence Magyar Denis Stogl @@ -22,6 +22,7 @@ rcutils realtime_tools sensor_msgs + urdf ament_cmake_gmock controller_manager diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp index 5eb0a9b9b6..fe0b32213a 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp @@ -25,6 +25,7 @@ #include "rclcpp/qos.hpp" #include "rclcpp/time.hpp" #include "std_msgs/msg/header.hpp" +#include "urdf/model.h" namespace rclcpp_lifecycle { @@ -163,6 +164,18 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_configure( fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return CallbackReturn::ERROR; } + + const std::string & urdf = get_robot_description(); + + is_model_loaded_ = !urdf.empty() && model_.initString(urdf); + if (!is_model_loaded_) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to parse robot description. Will publish all the interfaces with '%s', '%s' and '%s'", + HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT); + } + return CallbackReturn::SUCCESS; } @@ -249,7 +262,12 @@ bool JointStateBroadcaster::init_joint_data() const auto & interfaces_and_values = name_ifv.second; if (has_any_key(interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT})) { - joint_names_.push_back(name_ifv.first); + if ( + !params_.use_urdf_to_filter || !params_.joints.empty() || !is_model_loaded_ || + model_.getJoint(name_ifv.first)) + { + joint_names_.push_back(name_ifv.first); + } } } diff --git a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml index 8f0d4da6c5..c8ca928a12 100644 --- a/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml +++ b/joint_state_broadcaster/src/joint_state_broadcaster_parameters.yaml @@ -39,3 +39,10 @@ joint_state_broadcaster: type: string, default_value: "effort", } + use_urdf_to_filter: { + type: bool, + default_value: true, + description: "Uses the robot_description to filter the ``joint_states`` topic. + If true, the broadcaster will publish the data of the joints present in the URDF alone. + If false, the broadcaster will publish the data of any interface that has type ``position``, ``velocity``, or ``effort``." + } diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index 7400d6bbb9..877d199419 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -28,6 +28,7 @@ #include "rclcpp/executor.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" #include "test_joint_state_broadcaster.hpp" using hardware_interface::HW_IF_EFFORT; @@ -61,15 +62,17 @@ void JointStateBroadcasterTest::TearDown() { state_broadcaster_.reset(nullptr); void JointStateBroadcasterTest::SetUpStateBroadcaster( const std::vector & joint_names, const std::vector & interfaces) { - init_broadcaster_and_set_parameters(joint_names, interfaces); + init_broadcaster_and_set_parameters("", joint_names, interfaces); assign_state_interfaces(joint_names, interfaces); } void JointStateBroadcasterTest::init_broadcaster_and_set_parameters( - const std::vector & joint_names, const std::vector & interfaces) + const std::string & robot_description, const std::vector & joint_names, + const std::vector & interfaces) { const auto result = state_broadcaster_->init( - "joint_state_broadcaster", "", 0, "", state_broadcaster_->define_custom_node_options()); + "joint_state_broadcaster", robot_description, 0, "", + state_broadcaster_->define_custom_node_options()); ASSERT_EQ(result, controller_interface::return_type::OK); state_broadcaster_->get_node()->set_parameter({"joints", joint_names}); @@ -262,6 +265,212 @@ TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter) ElementsAreArray(interface_names_)); } +TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF) +{ + const std::vector JOINT_NAMES = {}; + const std::vector IF_NAMES = {interface_names_[0]}; + init_broadcaster_and_set_parameters("", JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // 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_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription) +{ + const std::vector JOINT_NAMES = {}; + const std::vector IF_NAMES = {interface_names_[0]}; + + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + const std::vector joint_in_urdf({"joint1", "joint2"}); + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // 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_in_urdf.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_in_urdf)); + 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 and it will have the data of all the interfaces + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); + 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_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces) +{ + const std::vector JOINT_NAMES = {"joint1"}; + const std::vector IF_NAMES = {}; + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + const std::vector joint_in_urdf({"joint1", "joint2"}); + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // 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_in_urdf.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_in_urdf)); + 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 and it will have the data of all the interfaces + const auto & dynamic_joint_state_msg = + state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_; + ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(joint_names_.size())); + ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(joint_names_.size())); + 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_)); +} + +TEST_F(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces) +{ + const std::vector JOINT_NAMES = {"joint1"}; + const std::vector IF_NAMES = interface_names_; + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::hardware_resources + ros2_control_test_assets::urdf_tail; + init_broadcaster_and_set_parameters(urdf_to_test, JOINT_NAMES, IF_NAMES); + assign_state_interfaces(JOINT_NAMES, IF_NAMES); + + // 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, SizeIs(JOINT_NAMES.size() * IF_NAMES.size())); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // 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 and it will have the data of all the interfaces + 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_)); +} + TEST_F(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter) { const std::vector JOINT_NAMES = {"joint1"}; @@ -426,7 +635,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing) const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; - init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]}); + init_broadcaster_and_set_parameters("", JOINT_NAMES, {interface_names_[0], interface_names_[1]}); // assign state with interfaces which are not set in parameters --> We should actually not assign // anything because CM will also not do that @@ -444,7 +653,7 @@ TEST_F(JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing) const std::vector JOINT_NAMES = {joint_names_[0], joint_names_[1]}; const std::vector IF_NAMES = {interface_names_[0], interface_names_[1]}; - init_broadcaster_and_set_parameters(JOINT_NAMES, {interface_names_[0], interface_names_[1]}); + init_broadcaster_and_set_parameters("", JOINT_NAMES, {interface_names_[0], interface_names_[1]}); // Manually assign existing interfaces --> one we need is missing std::vector state_ifs; diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp index 3e54116d5c..0b4c50e89e 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.hpp @@ -35,6 +35,10 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithJointsAndNoInterfaces); + FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithJointsAndInterfaces); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter); FRIEND_TEST(JointStateBroadcasterTest, ActivateDeactivateTestTwoJointsOneInterface); FRIEND_TEST(JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces); @@ -60,8 +64,8 @@ class JointStateBroadcasterTest : public ::testing::Test const std::vector & interfaces = {}); void init_broadcaster_and_set_parameters( - const std::vector & joint_names = {}, - const std::vector & interfaces = {}); + const std::string & robot_description, const std::vector & joint_names, + const std::vector & interfaces); void assign_state_interfaces( const std::vector & joint_names = {}, diff --git a/joint_trajectory_controller/CHANGELOG.rst b/joint_trajectory_controller/CHANGELOG.rst index 043f71623e..8b35a6986c 100644 --- a/joint_trajectory_controller/CHANGELOG.rst +++ b/joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * [JTC] Refactor URDF Model parsing (`#1227 `_) diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml index 3da60aa2c5..c8b670dac4 100644 --- a/joint_trajectory_controller/package.xml +++ b/joint_trajectory_controller/package.xml @@ -1,7 +1,7 @@ joint_trajectory_controller - 4.12.0 + 4.14.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 9563568ad5..e4923604fd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -137,7 +137,7 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } @@ -593,7 +593,7 @@ void JointTrajectoryController::query_state_service( { const auto logger = get_node()->get_logger(); // Preconditions - if (get_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + if (get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR(logger, "Can't sample trajectory. Controller is not active."); response->success = false; @@ -1112,7 +1112,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback( RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); // Precondition: Running controller - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR( get_node()->get_logger(), "Can't accept new action goals. Controller is not running."); diff --git a/parallel_gripper_controller/CHANGELOG.rst b/parallel_gripper_controller/CHANGELOG.rst index 56f5b21426..f8e7c77ddb 100644 --- a/parallel_gripper_controller/CHANGELOG.rst +++ b/parallel_gripper_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package parallel_gripper_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * [GripperController] Fix failing tests (`#1210 `_) diff --git a/parallel_gripper_controller/CMakeLists.txt b/parallel_gripper_controller/CMakeLists.txt index 75380a953f..cb0f4fafc9 100644 --- a/parallel_gripper_controller/CMakeLists.txt +++ b/parallel_gripper_controller/CMakeLists.txt @@ -48,9 +48,8 @@ if(BUILD_TESTING) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_parallel_gripper_action_controllers - test/test_load_parallel_gripper_action_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/gripper_action_controller_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_parallel_gripper_action_controllers test/test_load_parallel_gripper_action_controller.cpp) target_include_directories(test_load_parallel_gripper_action_controllers PRIVATE include) ament_target_dependencies(test_load_parallel_gripper_action_controllers controller_manager diff --git a/parallel_gripper_controller/package.xml b/parallel_gripper_controller/package.xml index 2bceb44069..055b5311b7 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.12.0 + 4.14.0 The parallel_gripper_controller package Bence Magyar diff --git a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp index 35a10a8c31..6453539fa0 100644 --- a/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp +++ b/parallel_gripper_controller/test/test_load_parallel_gripper_action_controller.cpp @@ -27,12 +27,15 @@ TEST(TestLoadGripperActionControllers, load_controller) 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) + "/gripper_action_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_gripper_action_position_controller", - "parallel_gripper_action_controller/GripperActionController"), - nullptr); + cm.set_parameter({"test_gripper_action_position_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_gripper_action_position_controller.type", + "parallel_gripper_action_controller/GripperActionController"}); + + ASSERT_NE(cm.load_controller("test_gripper_action_position_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/pid_controller/CHANGELOG.rst b/pid_controller/CHANGELOG.rst index c7a23dcba0..08e37d0836 100644 --- a/pid_controller/CHANGELOG.rst +++ b/pid_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package pid_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) 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 335f48f91c..9ef1964cd3 100644 --- a/pid_controller/package.xml +++ b/pid_controller/package.xml @@ -2,7 +2,7 @@ pid_controller - 4.12.0 + 4.14.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.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..9e6a7ef04c 100644 --- a/pid_controller/test/test_pid_controller_preceding.cpp +++ b/pid_controller/test/test_pid_controller_preceding.cpp @@ -90,6 +90,24 @@ TEST_F(PidControllerTest, check_exported_interfaces) ++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/position_controllers/CHANGELOG.rst b/position_controllers/CHANGELOG.rst index 95bba79c9d..17db75c10b 100644 --- a/position_controllers/CHANGELOG.rst +++ b/position_controllers/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package position_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/position_controllers/package.xml b/position_controllers/package.xml index b3a3fc6ce8..e79dd05059 100644 --- a/position_controllers/package.xml +++ b/position_controllers/package.xml @@ -1,7 +1,7 @@ position_controllers - 4.12.0 + 4.14.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 762ba806ba..36335db719 100644 --- a/range_sensor_broadcaster/CHANGELOG.rst +++ b/range_sensor_broadcaster/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package range_sensor_broadcaster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/range_sensor_broadcaster/CMakeLists.txt b/range_sensor_broadcaster/CMakeLists.txt index d7002d1b17..6f21607c9c 100644 --- a/range_sensor_broadcaster/CMakeLists.txt +++ b/range_sensor_broadcaster/CMakeLists.txt @@ -59,9 +59,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_range_sensor_broadcaster - test/test_load_range_sensor_broadcaster.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/range_sensor_broadcaster_params.yaml) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_range_sensor_broadcaster test/test_load_range_sensor_broadcaster.cpp) target_include_directories(test_load_range_sensor_broadcaster PRIVATE include) target_link_libraries(test_load_range_sensor_broadcaster range_sensor_broadcaster diff --git a/range_sensor_broadcaster/package.xml b/range_sensor_broadcaster/package.xml index 8ad0290ff7..433120ca8d 100644 --- a/range_sensor_broadcaster/package.xml +++ b/range_sensor_broadcaster/package.xml @@ -2,7 +2,7 @@ range_sensor_broadcaster - 4.12.0 + 4.14.0 Controller to publish readings of Range sensors. Bence Magyar Florent Chretien diff --git a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp index b416ac4ef9..11aae67f5d 100644 --- a/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp +++ b/range_sensor_broadcaster/test/test_load_range_sensor_broadcaster.cpp @@ -33,11 +33,14 @@ TEST(TestLoadRangeSensorBroadcaster, load_controller) 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) + "/range_sensor_broadcaster_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_range_sensor_broadcaster", "range_sensor_broadcaster/RangeSensorBroadcaster"), - nullptr); + cm.set_parameter({"test_range_sensor_broadcaster.params_file", test_file_path}); + cm.set_parameter( + {"test_range_sensor_broadcaster.type", "range_sensor_broadcaster/RangeSensorBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_range_sensor_broadcaster"), nullptr); } int main(int argc, char ** argv) diff --git a/ros2_controllers/CHANGELOG.rst b/ros2_controllers/CHANGELOG.rst index 52d97ab7d2..70fdee135d 100644 --- a/ros2_controllers/CHANGELOG.rst +++ b/ros2_controllers/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package ros2_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- diff --git a/ros2_controllers/package.xml b/ros2_controllers/package.xml index a01eb0921e..6ff486a1d8 100644 --- a/ros2_controllers/package.xml +++ b/ros2_controllers/package.xml @@ -1,7 +1,7 @@ ros2_controllers - 4.12.0 + 4.14.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 2cc1bb37dd..5f2afedd10 100644 --- a/ros2_controllers_test_nodes/CHANGELOG.rst +++ b/ros2_controllers_test_nodes/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package ros2_controllers_test_nodes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- +* Fix deprecation warning in paramater declaration (`#1280 `_) +* Contributors: Sanjeev + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- diff --git a/ros2_controllers_test_nodes/package.xml b/ros2_controllers_test_nodes/package.xml index 12388322bc..0b30bdd7ad 100644 --- a/ros2_controllers_test_nodes/package.xml +++ b/ros2_controllers_test_nodes/package.xml @@ -2,7 +2,7 @@ ros2_controllers_test_nodes - 4.12.0 + 4.14.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 394c11dab5..071e90ca0b 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.12.0", + version="4.14.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 d6e70f3a47..4cd8b487d8 100644 --- a/rqt_joint_trajectory_controller/CHANGELOG.rst +++ b/rqt_joint_trajectory_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package rqt_joint_trajectory_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- +* Fix bug for displaying all controllers (`#1259 `_) +* Contributors: Francisco Martín Rico + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- diff --git a/rqt_joint_trajectory_controller/package.xml b/rqt_joint_trajectory_controller/package.xml index 4f5674760d..c5973fd96e 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.12.0 + 4.14.0 Graphical frontend for interacting with joint_trajectory_controller instances. Bence Magyar diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 4cc6c901af..5b27c2c832 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -238,8 +238,11 @@ def _update_jtc_list(self): # for _all_ their joints running_jtc = self._running_jtc_info() if running_jtc and not self._robot_joint_limits: + self._robot_joint_limits = {} for jtc_info in running_jtc: - self._robot_joint_limits = get_joint_limits(self._node, _jtc_joint_names(jtc_info)) + self._robot_joint_limits.update( + get_joint_limits(self._node, _jtc_joint_names(jtc_info)) + ) valid_jtc = [] if self._robot_joint_limits: for jtc_info in running_jtc: diff --git a/rqt_joint_trajectory_controller/setup.py b/rqt_joint_trajectory_controller/setup.py index 60d3944d61..6c390b22e9 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.12.0", + version="4.14.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 04f10c3158..ab57a3b720 100644 --- a/steering_controllers_library/CHANGELOG.rst +++ b/steering_controllers_library/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package steering_controllers_library ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- +* fix(steering-odometry): handle infinite turning radius properly (`#1285 `_) +* Contributors: Rein Appeldoorn + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/steering_controllers_library/package.xml b/steering_controllers_library/package.xml index 3d300ae22e..41f7e3703d 100644 --- a/steering_controllers_library/package.xml +++ b/steering_controllers_library/package.xml @@ -2,7 +2,7 @@ steering_controllers_library - 4.12.0 + 4.14.0 Package for steering robot configurations including odometry and interfaces. Apache License 2.0 Bence Magyar diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index ba431faf33..824ec86f59 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; } diff --git a/tricycle_controller/CHANGELOG.rst b/tricycle_controller/CHANGELOG.rst index 890bdbb026..82721a5d1a 100644 --- a/tricycle_controller/CHANGELOG.rst +++ b/tricycle_controller/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package tricycle_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +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 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/tricycle_controller/CMakeLists.txt b/tricycle_controller/CMakeLists.txt index e264c1f258..6834b753a0 100644 --- a/tricycle_controller/CMakeLists.txt +++ b/tricycle_controller/CMakeLists.txt @@ -76,10 +76,8 @@ if(BUILD_TESTING) tf2_msgs ) - add_rostest_with_parameters_gmock(test_load_tricycle_controller - test/test_load_tricycle_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_tricycle_controller test/test_load_tricycle_controller.cpp) target_link_libraries(test_load_tricycle_controller tricycle_controller ) diff --git a/tricycle_controller/package.xml b/tricycle_controller/package.xml index e7d8c3438a..0fea2baab6 100644 --- a/tricycle_controller/package.xml +++ b/tricycle_controller/package.xml @@ -2,7 +2,7 @@ tricycle_controller - 4.12.0 + 4.14.0 Controller for a tricycle drive mobile base Bence Magyar Tony Najjar diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 94ff63e659..ec7ca7bd5e 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -86,7 +86,7 @@ InterfaceConfiguration TricycleController::state_interface_configuration() const controller_interface::return_type TricycleController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (get_state().id() == State::PRIMARY_STATE_INACTIVE) + if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE) { if (!is_halted) { diff --git a/tricycle_controller/test/test_load_tricycle_controller.cpp b/tricycle_controller/test/test_load_tricycle_controller.cpp index 486c04515b..7f7f0e6fed 100644 --- a/tricycle_controller/test/test_load_tricycle_controller.cpp +++ b/tricycle_controller/test/test_load_tricycle_controller.cpp @@ -33,10 +33,13 @@ TEST(TestLoadTricycleController, load_controller) 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) + "/config/test_tricycle_controller.yaml"; - ASSERT_NE( - cm.load_controller("test_tricycle_controller", "tricycle_controller/TricycleController"), - nullptr); + cm.set_parameter({"test_tricycle_controller.params_file", test_file_path}); + cm.set_parameter({"test_tricycle_controller.type", "tricycle_controller/TricycleController"}); + + ASSERT_NE(cm.load_controller("test_tricycle_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/tricycle_steering_controller/CHANGELOG.rst b/tricycle_steering_controller/CHANGELOG.rst index 6966dce4dc..697878ac81 100644 --- a/tricycle_steering_controller/CHANGELOG.rst +++ b/tricycle_steering_controller/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package tricycle_steering_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- +* Fixes tests to work with use_global_arguments NodeOptions parameter (`#1256 `_) +* Contributors: Sai Kishor Kothakota + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Add missing includes (`#1226 `_) diff --git a/tricycle_steering_controller/CMakeLists.txt b/tricycle_steering_controller/CMakeLists.txt index 92c2782092..24b4cd1a22 100644 --- a/tricycle_steering_controller/CMakeLists.txt +++ b/tricycle_steering_controller/CMakeLists.txt @@ -57,10 +57,8 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) - add_rostest_with_parameters_gmock(test_load_tricycle_steering_controller - test/test_load_tricycle_steering_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/test/tricycle_steering_controller_params.yaml - ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_tricycle_steering_controller test/test_load_tricycle_steering_controller.cpp) ament_target_dependencies(test_load_tricycle_steering_controller controller_manager hardware_interface diff --git a/tricycle_steering_controller/package.xml b/tricycle_steering_controller/package.xml index 50340a466b..5bf5a2d283 100644 --- a/tricycle_steering_controller/package.xml +++ b/tricycle_steering_controller/package.xml @@ -2,7 +2,7 @@ tricycle_steering_controller - 4.12.0 + 4.14.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_load_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp index 210153eef8..d3579c902f 100644 --- a/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_load_tricycle_steering_controller.cpp @@ -29,12 +29,15 @@ TEST(TestLoadTricycleSteeringController, load_controller) 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) + "/tricycle_steering_controller_params.yaml"; - ASSERT_NE( - cm.load_controller( - "test_tricycle_steering_controller", - "tricycle_steering_controller/TricycleSteeringController"), - nullptr); + cm.set_parameter({"test_tricycle_steering_controller.params_file", test_file_path}); + cm.set_parameter( + {"test_tricycle_steering_controller.type", + "tricycle_steering_controller/TricycleSteeringController"}); + + ASSERT_NE(cm.load_controller("test_tricycle_steering_controller"), nullptr); } int main(int argc, char ** argv) diff --git a/velocity_controllers/CHANGELOG.rst b/velocity_controllers/CHANGELOG.rst index 9d52d6fd25..5a662df92b 100644 --- a/velocity_controllers/CHANGELOG.rst +++ b/velocity_controllers/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package velocity_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +4.14.0 (2024-09-11) +------------------- + +4.13.0 (2024-08-22) +------------------- + +4.12.1 (2024-08-14) +------------------- + 4.12.0 (2024-07-23) ------------------- * Unused header cleanup (`#1199 `_) diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 8a3385b76e..e7ebdcc827 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -1,7 +1,7 @@ velocity_controllers - 4.12.0 + 4.14.0 Generic controller for forwarding commands. Bence Magyar Jordan Palacios