diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 01695888..f7ee2171 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -4,6 +4,7 @@ on: pull_request: branches: - humble + workflow_dispatch: schedule: - cron: "0 0 * * 0" @@ -14,12 +15,12 @@ jobs: runs-on: ${{ matrix.os }} strategy: matrix: - os: [ubuntu-latest] - fri_version: [1.11, 1.14, 1.15, 1.16, 2.5] + os: [ubuntu-22.04] + fri_version: [1.11, 1.14, 1.15, 1.16, 2.5, 2.7] steps: - uses: ros-tooling/setup-ros@v0.7 - uses: ros-tooling/action-ros-ci@v0.2 with: package-name: lbr_fri_ros2_stack target-ros2-distro: humble - vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/dev-humble-launch/lbr_fri_ros2_stack/repos-fri-${{ matrix.fri_version }}.yaml + vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${{ matrix.fri_version }}.yaml diff --git a/README.md b/README.md index e585670e..b9cbd2d4 100644 --- a/README.md +++ b/README.md @@ -85,10 +85,16 @@ If you enjoyed using this repository for your work, we would really appreciate ## Acknowledgements ### Open Source Contributors -We would like to acknowledge all open source contributors 🚀 +We would like to acknowledge all contributors 🚀 + +**lbr_fri_ros2_stack** [![lbr_fri_ros2_stack contributors](https://contrib.rocks/image?repo=lbr-stack/lbr_fri_ros2_stack&max=20)](https://github.com/lbr-stack/lbr_fri_ros2_stack/graphs/contributors) +**fri** + +[![fri contributors](https://contrib.rocks/image?repo=lbr-stack/fri&max=20)](https://github.com/lbr-stack/fri/graphs/contributors) + ### Organizations and Grants We would further like to acknowledge following supporters: diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index 54dd024f..f8a74694 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -11,7 +11,7 @@ lbr_description - FRIClient + fri_client_sdk geometry_msgs kdl_parser lbr_fri_idl diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 751e9dae..24b6f2c3 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -10,7 +10,7 @@ ament_cmake control_msgs - FRIClient + fri_client_sdk lbr_fri_idl lbr_fri_ros2 rclcpp diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp index e44d5dd3..debb2e86 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp @@ -61,7 +61,7 @@ struct EnumMaps { case KUKA::FRI::EClientCommandMode::POSITION: return "POSITION"; #endif -#if FRICLIENT_VERSION_MAJOR == 2 +#if FRICLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: return "JOINT_POSITION"; case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index 03328172..a009f5d5 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -14,7 +14,7 @@ eigen control_toolbox - FRIClient + fri_client_sdk kdl_parser lbr_fri_idl orocos_kdl_vendor diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index a32e7ae1..5c572fcc 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -21,7 +21,7 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod #if FRICLIENT_VERSION_MAJOR == 1 case KUKA::FRI::EClientCommandMode::POSITION: #endif -#if FRICLIENT_VERSION_MAJOR == 2 +#if FRICLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index c5445dbb..8ba1e04a 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -2,7 +2,7 @@ namespace lbr_fri_ros2 { CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameters) - : parameters_(command_guard_parameters){}; + : parameters_(command_guard_parameters), prev_measured_joint_position_init_(false){}; bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, const_idl_state_t_ref lbr_state) { diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 786fd479..0a81bfe6 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -19,7 +19,7 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command throw std::runtime_error(err); } #endif -#if FRICLIENT_VERSION_MAJOR == 2 +#if FRICLIENT_VERSION_MAJOR >= 2 if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) { std::string err = "Expected robot in " + diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 9f956ca0..3ab403cb 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -36,7 +36,7 @@ class TestCommandInterfaces : public ::testing::Test { #if FRICLIENT_VERSION_MAJOR == 1 case KUKA::FRI::EClientCommandMode::POSITION: #endif -#if FRICLIENT_VERSION_MAJOR == 2 +#if FRICLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { @@ -146,7 +146,7 @@ TEST_F(TestCommandInterfaces, TestPositionCommandInterface) { #if FRICLIENT_VERSION_MAJOR == 1 set_up(KUKA::FRI::EClientCommandMode::POSITION); #endif -#if FRICLIENT_VERSION_MAJOR == 2 +#if FRICLIENT_VERSION_MAJOR >= 2 set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION); #endif test_simple(); diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index c3679e7b..a072dd50 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -8,13 +8,13 @@ Apache License 2.0 ament_cmake + + lbr_fri_idl + lbr_fri_ros2 + lbr_ros2_control lbr_bringup - lbr_demos lbr_description - lbr_fri_idl - lbr_fri_ros2 - lbr_ros2_control ament_cmake diff --git a/lbr_fri_ros2_stack/repos-fri-2.7.yaml b/lbr_fri_ros2_stack/repos-fri-2.7.yaml new file mode 100644 index 00000000..90dcd423 --- /dev/null +++ b/lbr_fri_ros2_stack/repos-fri-2.7.yaml @@ -0,0 +1,13 @@ +repositories: + fri: + type: git + url: https://github.com/lbr-stack/fri + version: fri-2.7 + lbr_fri_idl: + type: git + url: https://github.com/lbr-stack/lbr_fri_idl + version: fri-2 + lbr_fri_ros2_stack: + type: git + url: https://github.com/lbr-stack/lbr_fri_ros2_stack + version: humble diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index cbb7d1db..3e9ec6cc 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -16,6 +16,8 @@ lbr_ros2_control::SystemInterface + ${system_parameters['hardware']['fri_client_sdk']['major_version']} + ${system_parameters['hardware']['fri_client_sdk']['minor_version']} ${system_parameters['hardware']['client_command_mode']} ${system_parameters['hardware']['port_id']} ${system_parameters['hardware']['remote_host']} @@ -103,7 +105,9 @@ - + + + diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml index ff044a74..91cbaeb6 100644 --- a/lbr_ros2_control/config/lbr_system_parameters.yaml +++ b/lbr_ros2_control/config/lbr_system_parameters.yaml @@ -1,5 +1,8 @@ # these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface hardware: + fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro + major_version: 1 + minor_version: 15 client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection @@ -11,7 +14,7 @@ hardware: pid_i_min: 0.0 # min integral value for the joint position command pid_antiwindup: false # enable antiwindup for the joint position command command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] - external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] + external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] open_loop: true # KUKA works the best in open_loop control mode diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index f8756b19..aace5b6a 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -31,10 +31,12 @@ namespace lbr_ros2_control { struct SystemInterfaceParameters { + uint8_t fri_client_sdk_major_version{1}; + uint8_t fri_client_sdk_minor_version{15}; #if FRICLIENT_VERSION_MAJOR == 1 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION}; #endif -#if FRICLIENT_VERSION_MAJOR == 2 +#if FRICLIENT_VERSION_MAJOR >= 2 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION}; #endif int32_t port_id{30200}; @@ -68,7 +70,12 @@ class SystemInterface : public hardware_interface::SystemInterface { protected: static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface"; +#if FRICLIENT_VERSION_MAJOR == 1 static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7; +#endif +#if FRICLIENT_VERSION_MAJOR >= 2 + static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 6; +#endif static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2; static constexpr uint8_t LBR_FRI_SENSORS = 2; static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12; diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 0e3cb57d..57fd12f8 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -9,7 +9,7 @@ ament_cmake - FRIClient + fri_client_sdk lbr_fri_idl lbr_fri_ros2 pluginlib diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 4dae0dc0..517818d0 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -320,12 +320,26 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &system_info) { try { + parameters_.fri_client_sdk_major_version = + std::stoul(system_info.hardware_parameters.at("fri_client_sdk_major_version")); + parameters_.fri_client_sdk_minor_version = + std::stoul(system_info.hardware_parameters.at("fri_client_sdk_minor_version")); + if (parameters_.fri_client_sdk_major_version != FRICLIENT_VERSION_MAJOR) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger(LOGGER_NAME), + lbr_fri_ros2::ColorScheme::ERROR + << "Expected FRI client SDK version '" << FRICLIENT_VERSION_MAJOR << "', got '" + << std::to_string(parameters_.fri_client_sdk_major_version) + << "'. Update lbr_system_parameters.yaml or compile against correct FRI version." + << lbr_fri_ros2::ColorScheme::ENDC); + return false; + } std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode"); if (client_command_mode == "position") { #if FRICLIENT_VERSION_MAJOR == 1 parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION; #endif -#if FRICLIENT_VERSION_MAJOR == 2 +#if FRICLIENT_VERSION_MAJOR >= 2 parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; #endif } else if (client_command_mode == "torque") {