diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index 2696ca44..4c559905 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -8,8 +8,8 @@ #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "friClientVersion.h" #include "friLBRClient.h" -#include "friVersion.h" #include "lbr_fri_ros2/filters.hpp" #include "lbr_fri_ros2/formatting.hpp" diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index 41c10c61..7dcaa91d 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -10,9 +10,9 @@ #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "friClientVersion.h" #include "friLBRClient.h" #include "friLBRState.h" -#include "friVersion.h" #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_idl/msg/lbr_state.hpp" @@ -66,7 +66,7 @@ class CommandGuard { class SafeStopCommandGuard : public CommandGuard { public: SafeStopCommandGuard(const CommandGuardParameters &command_guard_parameters) - : CommandGuard(command_guard_parameters){}; + : CommandGuard(command_guard_parameters) {}; protected: virtual bool command_in_position_limits_(const_idl_command_t_ref lbr_command, diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp index debb2e86..f605d599 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp @@ -3,8 +3,8 @@ #include +#include "friClientVersion.h" #include "friLBRClient.h" -#include "friVersion.h" namespace lbr_fri_ros2 { struct ColorScheme { @@ -57,11 +57,11 @@ struct EnumMaps { switch (client_command_mode) { case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE: return "NO_COMMAND_MODE"; -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 case KUKA::FRI::EClientCommandMode::POSITION: return "POSITION"; #endif -#if FRICLIENT_VERSION_MAJOR >= 2 +#if FRI_CLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: return "JOINT_POSITION"; case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index 93c061dc..eab3b4a3 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -9,8 +9,8 @@ #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "friClientVersion.h" #include "friLBRClient.h" -#include "friVersion.h" #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_ros2/command_guard.hpp" diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index cdfc97a7..9191d642 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -6,8 +6,8 @@ #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "friClientVersion.h" #include "friLBRClient.h" -#include "friVersion.h" #include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_fri_ros2/filters.hpp" diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 5c572fcc..8a9bd587 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -18,10 +18,10 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), "Command guard variant '" << command_guard_variant.c_str() << "'"); switch (client_command_mode) { -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 case KUKA::FRI::EClientCommandMode::POSITION: #endif -#if FRICLIENT_VERSION_MAJOR >= 2 +#if FRI_CLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 26287bdf..b9eef3ef 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -8,7 +8,7 @@ PositionCommandInterface::PositionCommandInterface( void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) { -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 if (state.client_command_mode != KUKA::FRI::EClientCommandMode::POSITION) { std::string err = "Expected robot in '" + EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::POSITION) + @@ -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 FRI_CLIENT_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/src/interfaces/state.cpp b/lbr_fri_ros2/src/interfaces/state.cpp index 31137b88..7e49c9bd 100644 --- a/lbr_fri_ros2/src/interfaces/state.cpp +++ b/lbr_fri_ros2/src/interfaces/state.cpp @@ -6,7 +6,7 @@ StateInterface::StateInterface(const StateInterfaceParameters &state_interface_p void StateInterface::set_state(const_fri_state_t_ref state) { state_.client_command_mode = state.getClientCommandMode(); -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 std::memcpy(state_.commanded_joint_position.data(), state.getCommandedJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); #endif @@ -43,7 +43,7 @@ void StateInterface::set_state(const_fri_state_t_ref state) { void StateInterface::set_state_open_loop(const_fri_state_t_ref state, const_idl_joint_pos_t_ref joint_position) { state_.client_command_mode = state.getClientCommandMode(); -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 std::memcpy(state_.commanded_joint_position.data(), state.getCommandedJointPosition(), sizeof(double) * fri_state_t::NUMBER_OF_JOINTS); #endif diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp index 3ab403cb..127fd2ee 100644 --- a/lbr_fri_ros2/test/test_command_interfaces.cpp +++ b/lbr_fri_ros2/test/test_command_interfaces.cpp @@ -4,9 +4,9 @@ #include "friClientApplication.h" #include "friClientIf.h" +#include "friClientVersion.h" #include "friLBRClient.h" #include "friUdpConnection.h" -#include "friVersion.h" #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_ros2/interfaces/base_command.hpp" @@ -33,10 +33,10 @@ class TestCommandInterfaces : public ::testing::Test { void set_up(const KUKA::FRI::EClientCommandMode &client_command_mode) { switch (client_command_mode) { -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 case KUKA::FRI::EClientCommandMode::POSITION: #endif -#if FRICLIENT_VERSION_MAJOR >= 2 +#if FRI_CLIENT_VERSION_MAJOR >= 2 case KUKA::FRI::EClientCommandMode::JOINT_POSITION: #endif { @@ -143,10 +143,10 @@ class TestCommandInterfaces : public ::testing::Test { }; TEST_F(TestCommandInterfaces, TestPositionCommandInterface) { -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 set_up(KUKA::FRI::EClientCommandMode::POSITION); #endif -#if FRICLIENT_VERSION_MAJOR >= 2 +#if FRI_CLIENT_VERSION_MAJOR >= 2 set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION); #endif test_simple(); diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp index 8df3df4d..15fa351c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp @@ -15,8 +15,8 @@ #include "realtime_tools/realtime_publisher.h" #include "friClientIf.h" +#include "friClientVersion.h" #include "friLBRState.h" -#include "friVersion.h" #include "lbr_fri_idl/msg/lbr_state.hpp" #include "lbr_ros2_control/system_interface_type_values.hpp" 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 aace5b6a..08a9b41f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -15,8 +15,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "friClientVersion.h" #include "friLBRState.h" -#include "friVersion.h" #include "lbr_fri_idl/msg/lbr_command.hpp" #include "lbr_fri_idl/msg/lbr_state.hpp" @@ -33,10 +33,10 @@ 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 +#if FRI_CLIENT_VERSION_MAJOR == 1 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION}; #endif -#if FRICLIENT_VERSION_MAJOR >= 2 +#if FRI_CLIENT_VERSION_MAJOR >= 2 KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION}; #endif int32_t port_id{30200}; @@ -70,10 +70,10 @@ class SystemInterface : public hardware_interface::SystemInterface { protected: static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface"; -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7; #endif -#if FRICLIENT_VERSION_MAJOR >= 2 +#if FRI_CLIENT_VERSION_MAJOR >= 2 static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 6; #endif static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2; diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/lbr_state_broadcaster.cpp index 3ea4afaf..be79cc98 100644 --- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/lbr_state_broadcaster.cpp @@ -77,7 +77,7 @@ controller_interface::return_type LBRStateBroadcaster::update(const rclcpp::Time // joint related states std::for_each(joint_names_.begin(), joint_names_.end(), [&, idx = 0](const std::string &joint_name) mutable { -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 rt_state_publisher_ptr_->msg_.commanded_joint_position[idx] = state_interface_map_[joint_name][HW_IF_COMMANDED_JOINT_POSITION]; #endif @@ -133,7 +133,7 @@ void LBRStateBroadcaster::init_state_interface_map_() { void LBRStateBroadcaster::init_state_msg_() { rt_state_publisher_ptr_->msg_.client_command_mode = std::numeric_limits::quiet_NaN(); -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 rt_state_publisher_ptr_->msg_.commanded_joint_position.fill( std::numeric_limits::quiet_NaN()); #endif diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 517818d0..e058b7f0 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -110,7 +110,7 @@ std::vector SystemInterface::export_state_in state_interfaces.emplace_back(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_lbr_state_.measured_joint_position[i]); -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 state_interfaces.emplace_back(info_.joints[i].name, HW_IF_COMMANDED_JOINT_POSITION, &hw_lbr_state_.commanded_joint_position[i]); #endif @@ -324,11 +324,11 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & 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) { + if (parameters_.fri_client_sdk_major_version != FRI_CLIENT_VERSION_MAJOR) { RCLCPP_ERROR_STREAM( rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR - << "Expected FRI client SDK version '" << FRICLIENT_VERSION_MAJOR << "', got '" + << "Expected FRI client SDK version '" << FRI_CLIENT_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); @@ -336,10 +336,10 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & } std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode"); if (client_command_mode == "position") { -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION; #endif -#if FRICLIENT_VERSION_MAJOR >= 2 +#if FRI_CLIENT_VERSION_MAJOR >= 2 parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION; #endif } else if (client_command_mode == "torque") { @@ -405,7 +405,7 @@ void SystemInterface::nan_command_interfaces_() { void SystemInterface::nan_state_interfaces_() { // state interfaces of type double hw_lbr_state_.measured_joint_position.fill(std::numeric_limits::quiet_NaN()); -#if FRICLIENT_VERSION_MAJOR == 1 +#if FRI_CLIENT_VERSION_MAJOR == 1 hw_lbr_state_.commanded_joint_position.fill(std::numeric_limits::quiet_NaN()); #endif hw_lbr_state_.measured_torque.fill(std::numeric_limits::quiet_NaN());