diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4bca9576..03900f49 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -3,28 +3,38 @@ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Jazzy v2.x (TBD) -------------------------- -* ``lbr_demos``: Replace ``ament_target_dependencies`` with ``target_link_libraries``. -* ``lbr_fri_ros2``: +* ``lbr_bringup``: - * Replace ``ament_target_dependencies`` with ``target_link_libraries``. - * Provide default for ``StateInterfaceParameters`` to fix test. -* ``lbr_ros2_control``: Replace ``ament_target_dependencies`` with ``target_link_libraries``. + * Adds a dedicated ``namespace`` argument to mock / hardware launch files. + * Fix the MoveIt servo node name. +* ``lbr_demos``: -* ``lbr_bringup``: Adds a dedicated ``namespace`` argument to mock / hardware launch files. + * Replace ``ament_target_dependencies`` with ``target_link_libraries``. + * ``lbr_moveit``: Add missing ``pynput`` dependency and fix dataclasses. * ``lbr_description``: * Sets the ``k_velocity`` to fix the safety limits for effort client command modes. * Removes the controller-related parts from the ``*_description.xacro`` files, now named ``*.urdf.xacro``. * Fix tests. * Depend on ``ament_cmake_pytest`` for tests only. -* ``lbr_ros2_control``: Uses the joint limits as obtained from the joints inside the URDF, previously redundant in ```` tags. +* ``lbr_fri_ros2``: + + * Replace ``ament_target_dependencies`` with ``target_link_libraries``. + * Provide default for ``StateInterfaceParameters`` to fix test. +* ``lbr_ros2_control``: + + * Migrates to the new ``ros2_control`` interface handles: https://github.com/ros-controls/ros2_control/pull/2831. + * Uses the joint limits as obtained from the joints inside the URDF, previously redundant in ```` tags. + * Replace ``ament_target_dependencies`` with ``target_link_libraries``. * Related pull requests: + * MoveIt fix: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/379 * Namespace argument: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/344, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/353 * Effort limits: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/357 * URDF de-coupling: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/359 * Ament target dependencies: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/364 * Test fixes: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/367, https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/373 + * Interface migration: https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/378 Jazzy v2.4.3 (2025-12-09) -------------------------- 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 54e98da1..8c8c6e88 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -2,6 +2,7 @@ #define LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_ #include +#include #include #include #include @@ -55,61 +56,177 @@ class SystemInterface : public hardware_interface::SystemInterface { bool open_loop{true}; }; - struct CommandKeys { - lbr_fri_ros2::jnt_name_array_t joint_position, torque; - lbr_fri_ros2::cart_name_array_t wrench; + struct CommandInterfaceHandles { + std::array + joint_position, torque; + std::array wrench; - void populate_keys(const hardware_interface::HardwareInfo &info) { + void populate(const hardware_interface::SystemInterface &si) { + const auto &info = si.get_hardware_info(); for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - auto joint_name = info.joints[i].name; - joint_position[i] = joint_name + "/" + hardware_interface::HW_IF_POSITION; - torque[i] = joint_name + "/" + hardware_interface::HW_IF_EFFORT; + const auto &joint_name = info.joints[i].name; + joint_position[i] = + si.get_command_interface_handle(joint_name + "/" + hardware_interface::HW_IF_POSITION); + torque[i] = + si.get_command_interface_handle(joint_name + "/" + hardware_interface::HW_IF_EFFORT); + } + wrench[0] = + si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_X); + wrench[1] = + si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Y); + wrench[2] = + si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Z); + wrench[3] = + si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_X); + wrench[4] = + si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Y); + wrench[5] = + si.get_command_interface_handle(std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Z); + } + + void nan_interfaces() const { + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + std::ignore = joint_position[i]->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = torque[i]->set_value(std::numeric_limits::quiet_NaN()); + } + for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + std::ignore = wrench[i]->set_value(std::numeric_limits::quiet_NaN()); + } + } + + void pull(lbr_fri_idl::msg::LBRCommand &lbr_command) const { + // populate command message + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + lbr_command.joint_position[i] = joint_position[i]->get_optional().value(); + lbr_command.torque[i] = torque[i]->get_optional().value(); + } + for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { + lbr_command.wrench[i] = wrench[i]->get_optional().value(); } - wrench[0] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_X; - wrench[1] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Y; - wrench[2] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_FORCE_Z; - wrench[3] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_X; - wrench[4] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Y; - wrench[5] = std::string(HW_IF_WRENCH_PREFIX) + "/" + HW_IF_TORQUE_Z; } }; - struct StateKeys { + struct StateInterfaceHandles { #if FRI_CLIENT_VERSION_MAJOR == 1 - lbr_fri_ros2::jnt_name_array_t commanded_joint_position; + std::array + commanded_joint_position; #endif - lbr_fri_ros2::jnt_name_array_t commanded_torque, ipo_joint_position, position, external_torque, - effort, velocity; - std::string sample_time, session_state, connection_quality, safety_state, operation_mode, - drive_state, client_command_mode, overlay_type, control_mode, time_stamp_sec, - time_stamp_nano_sec, tracking_performance; + std::array + commanded_torque, ipo_joint_position, position, external_torque, effort, velocity; + hardware_interface::StateInterface::SharedPtr sample_time, session_state, connection_quality, + safety_state, operation_mode, drive_state, client_command_mode, overlay_type, control_mode, + time_stamp_sec, time_stamp_nano_sec, tracking_performance; - void populate_keys(const hardware_interface::HardwareInfo &info) { + void populate(const hardware_interface::SystemInterface &si) { + const auto &info = si.get_hardware_info(); for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - auto joint_name = info.joints[i].name; + const auto &joint_name = info.joints[i].name; #if FRI_CLIENT_VERSION_MAJOR == 1 - commanded_joint_position[i] = joint_name + "/" + HW_IF_COMMANDED_JOINT_POSITION; + commanded_joint_position[i] = + si.get_state_interface_handle(joint_name + "/" + HW_IF_COMMANDED_JOINT_POSITION); #endif - commanded_torque[i] = joint_name + "/" + HW_IF_COMMANDED_TORQUE; - ipo_joint_position[i] = joint_name + "/" + HW_IF_IPO_JOINT_POSITION; - position[i] = joint_name + "/" + hardware_interface::HW_IF_POSITION; - external_torque[i] = joint_name + "/" + HW_IF_EXTERNAL_TORQUE; - effort[i] = joint_name + "/" + hardware_interface::HW_IF_EFFORT; - velocity[i] = joint_name + "/" + hardware_interface::HW_IF_VELOCITY; + commanded_torque[i] = + si.get_state_interface_handle(joint_name + "/" + HW_IF_COMMANDED_TORQUE); + ipo_joint_position[i] = + si.get_state_interface_handle(joint_name + "/" + HW_IF_IPO_JOINT_POSITION); + position[i] = + si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_POSITION); + external_torque[i] = + si.get_state_interface_handle(joint_name + "/" + HW_IF_EXTERNAL_TORQUE); + effort[i] = + si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_EFFORT); + velocity[i] = + si.get_state_interface_handle(joint_name + "/" + hardware_interface::HW_IF_VELOCITY); } - sample_time = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SAMPLE_TIME; - session_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SESSION_STATE; - connection_quality = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CONNECTION_QUALITY; - safety_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_SAFETY_STATE; - operation_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_OPERATION_MODE; - drive_state = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_DRIVE_STATE; - client_command_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CLIENT_COMMAND_MODE; - overlay_type = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_OVERLAY_TYPE; - control_mode = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_CONTROL_MODE; - time_stamp_sec = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TIME_STAMP_SEC; - time_stamp_nano_sec = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TIME_STAMP_NANO_SEC; - tracking_performance = std::string(HW_IF_AUXILIARY_PREFIX) + "/" + HW_IF_TRACKING_PERFORMANCE; + sample_time = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SAMPLE_TIME); + session_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SESSION_STATE); + connection_quality = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_CONNECTION_QUALITY); + safety_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_SAFETY_STATE); + operation_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_OPERATION_MODE); + drive_state = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_DRIVE_STATE); + client_command_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + + "/" + HW_IF_CLIENT_COMMAND_MODE); + overlay_type = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_OVERLAY_TYPE); + control_mode = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_CONTROL_MODE); + time_stamp_sec = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + "/" + + HW_IF_TIME_STAMP_SEC); + time_stamp_nano_sec = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + + "/" + HW_IF_TIME_STAMP_NANO_SEC); + tracking_performance = si.get_state_interface_handle(std::string(HW_IF_AUXILIARY_PREFIX) + + "/" + HW_IF_TRACKING_PERFORMANCE); + } + + void nan_interfaces() const { + // joint state interfaces + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { + std::ignore = position[i]->set_value(std::numeric_limits::quiet_NaN()); +#if FRI_CLIENT_VERSION_MAJOR == 1 + std::ignore = + commanded_joint_position[i]->set_value(std::numeric_limits::quiet_NaN()); +#endif + std::ignore = effort[i]->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = commanded_torque[i]->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = external_torque[i]->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = ipo_joint_position[i]->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = velocity[i]->set_value(std::numeric_limits::quiet_NaN()); + } + std::ignore = sample_time->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = tracking_performance->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = session_state->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = connection_quality->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = safety_state->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = operation_mode->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = drive_state->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = client_command_mode->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = overlay_type->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = control_mode->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = time_stamp_sec->set_value(std::numeric_limits::quiet_NaN()); + std::ignore = time_stamp_nano_sec->set_value(std::numeric_limits::quiet_NaN()); + } + + void + push(const lbr_fri_idl::msg::LBRState &lbr_state, + const lbr_fri_idl::msg::LBRState::_measured_joint_position_type &velocity_estimate) const { + // set the joint state interfaces + for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { +#if FRI_CLIENT_VERSION_MAJOR == 1 + std::ignore = commanded_joint_position[i]->set_value(lbr_state.commanded_joint_position[i]); +#endif + std::ignore = commanded_torque[i]->set_value(lbr_state.commanded_torque[i]); + std::ignore = ipo_joint_position[i]->set_value(lbr_state.ipo_joint_position[i]); + std::ignore = position[i]->set_value(lbr_state.measured_joint_position[i]); + std::ignore = external_torque[i]->set_value(lbr_state.external_torque[i]); + std::ignore = effort[i]->set_value(lbr_state.measured_torque[i]); + std::ignore = velocity[i]->set_value(velocity_estimate[i]); + } + + // state interfaces without cast + std::ignore = sample_time->set_value(lbr_state.sample_time); + std::ignore = tracking_performance->set_value(lbr_state.tracking_performance); + + // state interfaces with cast + std::ignore = session_state->set_value(static_cast(lbr_state.session_state)); + std::ignore = + connection_quality->set_value(static_cast(lbr_state.connection_quality)); + std::ignore = safety_state->set_value(static_cast(lbr_state.safety_state)); + std::ignore = operation_mode->set_value(static_cast(lbr_state.operation_mode)); + std::ignore = drive_state->set_value(static_cast(lbr_state.drive_state)); + std::ignore = + client_command_mode->set_value(static_cast(lbr_state.client_command_mode)); + std::ignore = overlay_type->set_value(static_cast(lbr_state.overlay_type)); + std::ignore = control_mode->set_value(static_cast(lbr_state.control_mode)); + std::ignore = time_stamp_sec->set_value(static_cast(lbr_state.time_stamp_sec)); + std::ignore = + time_stamp_nano_sec->set_value(static_cast(lbr_state.time_stamp_nano_sec)); } }; @@ -173,6 +290,9 @@ class SystemInterface : public hardware_interface::SystemInterface { std::shared_ptr async_client_ptr_; std::unique_ptr app_ptr_; + // session state tracking + KUKA::FRI::ESessionState previous_session_state_; + // velocity computation lbr_fri_idl::msg::LBRState::_measured_joint_position_type last_measured_joint_position_, velocity_; @@ -189,9 +309,9 @@ class SystemInterface : public hardware_interface::SystemInterface { lbr_fri_idl::msg::LBRCommand lbr_command_; lbr_fri_idl::msg::LBRState lbr_state_; - // keys for command / state interfaces - CommandKeys command_keys_; - StateKeys state_keys_; + // interface handles for commands / states + CommandInterfaceHandles command_if_handles_; + StateInterfaceHandles state_if_handles_; }; } // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_ diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 798cdddb..a4ccf48b 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -55,10 +55,6 @@ SystemInterface::on_init(const hardware_interface::HardwareComponentInterfacePar return controller_interface::CallbackReturn::ERROR; } - // populate the keys - command_keys_.populate_keys(info_); - state_keys_.populate_keys(info_); - // perform verifications if (!verify_number_of_joints_()) { return controller_interface::CallbackReturn::ERROR; @@ -104,6 +100,10 @@ SystemInterface::on_configure(const rclcpp_lifecycle::State &) { } controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_lifecycle::State &) { + // populate the interface handles + command_if_handles_.populate(*this); + state_if_handles_.populate(*this); + // nan all command and state interfaces (this might be removed in the future with ros2_control // handling them now) nan_command_interfaces_(); @@ -160,6 +160,8 @@ controller_interface::CallbackReturn SystemInterface::on_activate(const rclcpp_l } std::this_thread::sleep_for(std::chrono::seconds(1)); } + // initialize the previous session state + previous_session_state_ = static_cast(state.session_state); return controller_interface::CallbackReturn::SUCCESS; } @@ -191,9 +193,8 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim } // exit once robot exits COMMANDING_ACTIVE (for safety) - if (exit_commanding_active_( - static_cast(get_state(state_keys_.session_state)), - static_cast(lbr_state_.session_state))) { + auto current_session_state = static_cast(lbr_state_.session_state); + if (exit_commanding_active_(previous_session_state_, current_session_state)) { RCLCPP_ERROR_STREAM(get_node()->get_logger(), lbr_fri_ros2::ColorScheme::ERROR << "LBR left COMMANDING_ACTIVE. Please re-run lbr_bringup" @@ -202,58 +203,28 @@ hardware_interface::return_type SystemInterface::read(const rclcpp::Time & /*tim app_ptr_->close_udp_socket(); return hardware_interface::return_type::ERROR; } + previous_session_state_ = current_session_state; - // set the joint state interfaces - for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { -#if FRI_CLIENT_VERSION_MAJOR == 1 - set_state(state_keys_.commanded_joint_position[i], lbr_state_.commanded_joint_position[i]); -#endif - set_state(state_keys_.commanded_torque[i], lbr_state_.commanded_torque[i]); - set_state(state_keys_.ipo_joint_position[i], lbr_state_.ipo_joint_position[i]); - set_state(state_keys_.position[i], lbr_state_.measured_joint_position[i]); - set_state(state_keys_.external_torque[i], lbr_state_.external_torque[i]); - set_state(state_keys_.effort[i], lbr_state_.measured_torque[i]); - set_state(state_keys_.velocity[i], velocity_[i]); - } - - // state interfaces without - set_state(state_keys_.sample_time, lbr_state_.sample_time); - set_state(state_keys_.tracking_performance, lbr_state_.tracking_performance); - - // state interfaces with cast - set_state(state_keys_.session_state, static_cast(lbr_state_.session_state)); - set_state(state_keys_.connection_quality, static_cast(lbr_state_.connection_quality)); - set_state(state_keys_.safety_state, static_cast(lbr_state_.safety_state)); - set_state(state_keys_.operation_mode, static_cast(lbr_state_.operation_mode)); - set_state(state_keys_.drive_state, static_cast(lbr_state_.drive_state)); - set_state(state_keys_.client_command_mode, static_cast(lbr_state_.client_command_mode)); - set_state(state_keys_.overlay_type, static_cast(lbr_state_.overlay_type)); - set_state(state_keys_.control_mode, static_cast(lbr_state_.control_mode)); - set_state(state_keys_.time_stamp_sec, static_cast(lbr_state_.time_stamp_sec)); - set_state(state_keys_.time_stamp_nano_sec, static_cast(lbr_state_.time_stamp_nano_sec)); - - // additional velocity state interface + // compute velocity compute_velocity_(); + + // set the joint state interfaces + state_if_handles_.push(lbr_state_, velocity_); + update_last_states_(); return hardware_interface::return_type::OK; } hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - if (static_cast(get_state(state_keys_.session_state)) != - KUKA::FRI::COMMANDING_ACTIVE) { + if (lbr_state_.session_state != KUKA::FRI::COMMANDING_ACTIVE) { return hardware_interface::return_type::OK; } // populate command message - for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - lbr_command_.joint_position[i] = get_command(command_keys_.joint_position[i]); - lbr_command_.torque[i] = get_command(command_keys_.torque[i]); - } - for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { - lbr_command_.wrench[i] = get_command(command_keys_.wrench[i]); - } + command_if_handles_.pull(lbr_command_); + // forward message to fri async_client_ptr_->get_command_interface()->buffer_command_target(lbr_command_); return hardware_interface::return_type::OK; } @@ -337,49 +308,9 @@ bool SystemInterface::parse_parameters_() { return true; } -void SystemInterface::nan_command_interfaces_() { - for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - set_command(command_keys_.joint_position[i], std::numeric_limits::quiet_NaN()); - set_command(command_keys_.torque[i], std::numeric_limits::quiet_NaN()); - } - for (std::size_t i = 0; i < lbr_fri_ros2::CARTESIAN_DOF; ++i) { - set_command(command_keys_.wrench[i], std::numeric_limits::quiet_NaN()); - } -} +void SystemInterface::nan_command_interfaces_() { command_if_handles_.nan_interfaces(); } -void SystemInterface::nan_state_interfaces_() { - // joint state interfaces - for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - set_state(state_keys_.position[i], std::numeric_limits::quiet_NaN()); -#if FRI_CLIENT_VERSION_MAJOR == 1 - set_state(state_keys_.commanded_joint_position[i], std::numeric_limits::quiet_NaN()); -#endif - set_state(state_keys_.effort[i], std::numeric_limits::quiet_NaN()); - set_state(state_keys_.commanded_torque[i], std::numeric_limits::quiet_NaN()); - set_state(state_keys_.external_torque[i], std::numeric_limits::quiet_NaN()); - set_state(state_keys_.ipo_joint_position[i], std::numeric_limits::quiet_NaN()); - set_state(state_keys_.velocity[i], std::numeric_limits::quiet_NaN()); - } - - // state interface without cast - set_state(state_keys_.sample_time, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.tracking_performance, std::numeric_limits::quiet_NaN()); - - // state interfaces with cast - set_state(state_keys_.session_state, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.connection_quality, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.safety_state, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.operation_mode, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.drive_state, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.client_command_mode, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.overlay_type, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.control_mode, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.time_stamp_sec, std::numeric_limits::quiet_NaN()); - set_state(state_keys_.time_stamp_nano_sec, std::numeric_limits::quiet_NaN()); - - // additional velocity state interface - velocity_.fill(std::numeric_limits::quiet_NaN()); -} +void SystemInterface::nan_state_interfaces_() { state_if_handles_.nan_interfaces(); } bool SystemInterface::verify_number_of_joints_() { if (info_.joints.size() != lbr_fri_ros2::N_JNTS) { @@ -567,10 +498,10 @@ void SystemInterface::nan_last_states_() { void SystemInterface::update_last_states_() { for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - last_measured_joint_position_[i] = get_state(state_keys_.position[i]); + last_measured_joint_position_[i] = lbr_state_.measured_joint_position[i]; } - last_time_stamp_sec_ = get_state(state_keys_.time_stamp_sec); - last_time_stamp_nano_sec_ = get_state(state_keys_.time_stamp_nano_sec); + last_time_stamp_sec_ = lbr_state_.time_stamp_sec; + last_time_stamp_nano_sec_ = lbr_state_.time_stamp_nano_sec; } void SystemInterface::compute_velocity_() { @@ -579,8 +510,8 @@ void SystemInterface::compute_velocity_() { return; } - auto time_stamp_sec = get_state(state_keys_.time_stamp_sec); - auto time_stamp_nano_sec = get_state(state_keys_.time_stamp_nano_sec); + const double time_stamp_sec = lbr_state_.time_stamp_sec; + const double time_stamp_nano_sec = lbr_state_.time_stamp_nano_sec; // state wasn't updated if (last_time_stamp_sec_ == time_stamp_sec && last_time_stamp_nano_sec_ == time_stamp_nano_sec) { @@ -589,8 +520,11 @@ void SystemInterface::compute_velocity_() { double dt = time_stamps_to_sec_(time_stamp_sec, time_stamp_nano_sec) - time_stamps_to_sec_(last_time_stamp_sec_, last_time_stamp_nano_sec_); + if (dt <= 0) { + return; + } for (std::size_t i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { - velocity_[i] = (get_state(state_keys_.position[i]) - last_measured_joint_position_[i]) / dt; + velocity_[i] = (lbr_state_.measured_joint_position[i] - last_measured_joint_position_[i]) / dt; } }