From 2171bd85ca36dba786a6580e570f2d000a7d3f06 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 22 Oct 2024 11:14:30 +0100 Subject: [PATCH] added a N_JNTS alias to types --- .../src/pose_control_node.cpp | 8 +++---- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 2 +- .../include/lbr_fri_ros2/ft_estimator.hpp | 4 ++-- lbr_fri_ros2/include/lbr_fri_ros2/types.hpp | 11 ++++++---- lbr_fri_ros2/src/filters.cpp | 9 ++++---- lbr_fri_ros2/src/ft_estimator.cpp | 3 +-- lbr_fri_ros2/src/kinematics.cpp | 22 ++++++++----------- .../controllers/twist_controller.hpp | 3 +-- .../src/controllers/admittance_controller.cpp | 10 ++++----- .../lbr_joint_position_command_controller.cpp | 8 +++---- .../src/controllers/lbr_state_broadcaster.cpp | 6 ++--- .../lbr_torque_command_controller.cpp | 16 +++++++------- .../lbr_wrench_command_controller.cpp | 12 +++++----- .../src/controllers/twist_controller.cpp | 12 +++++----- lbr_ros2_control/src/system_interface.cpp | 7 +++--- 15 files changed, 64 insertions(+), 69 deletions(-) diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp index 47602410..d3d7c048 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/pose_control_node.cpp @@ -54,7 +54,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { current_lbr_state_ = *lbr_state; double joint_position[KUKA::FRI::LBRState::NUMBER_OF_JOINTS]; - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { joint_position[i] = current_lbr_state_.measured_joint_position[i]; } current_pose_ = compute_fk_(joint_position); @@ -65,7 +65,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { if (current_lbr_state_.session_state == KUKA::FRI::COMMANDING_ACTIVE) { KDL::JntArray current_joint_positions(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { current_joint_positions(i) = current_lbr_state_.measured_joint_position[i]; } @@ -79,7 +79,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain_); KDL::JntArray joint_positions = KDL::JntArray(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (unsigned int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++i) { joint_positions(i) = position_array_ptr[i]; } @@ -131,7 +131,7 @@ class PoseControlNode : public LBRBasePositionCommandNode { { RCLCPP_ERROR(this->get_logger(), "Inverse kinematics failed."); } else { - for (unsigned int i = 0; i < result_joint_positions.data.size(); i++) { + for (unsigned int i = 0; i < result_joint_positions.data.size(); ++i) { joint_position_command.joint_position[i] = result_joint_positions(i); } } diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index 07dcff6d..6f62bdd6 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -121,7 +121,7 @@ struct PIDParameters { class JointPIDArray { protected: static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::JointPIDArray"; - using pid_array_t = std::array; + using pid_array_t = std::array; public: JointPIDArray() = delete; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index abd13f94..aa53d109 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -39,8 +39,8 @@ class FTEstimator { std::unique_ptr kinematics_ptr_; // force estimation - Eigen::Matrix jacobian_inv_; - Eigen::Matrix tau_ext_; + Eigen::Matrix jacobian_inv_; + Eigen::Matrix tau_ext_; Eigen::Matrix f_ext_; }; } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp index fbd2a610..faddc9eb 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/types.hpp @@ -11,20 +11,23 @@ #include "lbr_fri_idl/msg/lbr_state.hpp" namespace lbr_fri_ros2 { +// joint DoF alias +constexpr std::uint8_t N_JNTS = KUKA::FRI::LBRState::NUMBER_OF_JOINTS; + // joint positions, velocities, accelerations, torques etc. -using jnt_array_t = std::array; +using jnt_array_t = std::array; using jnt_array_t_ref = jnt_array_t &; using const_jnt_array_t_ref = const jnt_array_t &; // joint names -using jnt_name_array_t = std::array; +using jnt_name_array_t = std::array; using jnt_name_array_t_ref = jnt_name_array_t &; using const_jnt_name_array_t_ref = const jnt_name_array_t &; -// Cartesian dof +// Cartesian DoF constexpr std::uint8_t CARTESIAN_DOF = 6; -// Cartesian positions, velocities, accelerations, torques etc. +// Cartesian positions, velocities, accelerations, wrenches etc. using cart_array_t = std::array; using cart_array_t_ref = cart_array_t &; using const_cart_array_t_ref = const cart_array_t &; diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index 983835d2..eb842ccc 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -30,11 +30,10 @@ double ExponentialFilter::compute_alpha_(const double &cutoff_frequency, bool ExponentialFilter::validate_alpha_(const double &alpha) { return alpha <= 1. && alpha >= 0.; } void JointExponentialFilterArray::compute(const double *const current, jnt_array_t_ref previous) { - std::for_each(current, current + KUKA::FRI::LBRState::NUMBER_OF_JOINTS, - [&, i = 0](const auto ¤t_i) mutable { - previous[i] = exponential_filter_.compute(current_i, previous[i]); - ++i; - }); + std::for_each(current, current + N_JNTS, [&, i = 0](const auto ¤t_i) mutable { + previous[i] = exponential_filter_.compute(current_i, previous[i]); + ++i; + }); } void JointExponentialFilterArray::initialize(const double &cutoff_frequency, diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 05ba96aa..8f6a9d73 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -11,8 +11,7 @@ FTEstimator::FTEstimator(const std::string &robot_description, const std::string void FTEstimator::compute(const_jnt_array_t_ref measured_joint_position, const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext, const double &damping) { - tau_ext_ = Eigen::Map>( - external_torque.data()); + tau_ext_ = Eigen::Map>(external_torque.data()); auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position); jacobian_inv_ = pinv(jacobian.data, damping); f_ext_ = jacobian_inv_.transpose() * tau_ext_; diff --git a/lbr_fri_ros2/src/kinematics.cpp b/lbr_fri_ros2/src/kinematics.cpp index b7da0ebe..6cc56929 100644 --- a/lbr_fri_ros2/src/kinematics.cpp +++ b/lbr_fri_ros2/src/kinematics.cpp @@ -13,52 +13,48 @@ Kinematics::Kinematics(const std::string &robot_description, const std::string & RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } - if (chain_.getNrOfJoints() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (chain_.getNrOfJoints() != N_JNTS) { std::string err = "Invalid number of joints in chain."; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } jacobian_solver_ = std::make_unique(chain_); fk_solver_ = std::make_unique(chain_); - jacobian_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - q_.resize(KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + jacobian_.resize(N_JNTS); + q_.resize(N_JNTS); q_.data.setZero(); } const KDL::Jacobian &Kinematics::compute_jacobian(const_jnt_array_t_ref q) { - q_.data = - Eigen::Map>(q.data()); + q_.data = Eigen::Map>(q.data()); jacobian_solver_->JntToJac(q_, jacobian_); return jacobian_; } const KDL::Jacobian &Kinematics::compute_jacobian(const std::vector &q) { - if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (q.size() != N_JNTS) { std::string err = "Invalid number of joint positions."; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } - q_.data = - Eigen::Map>(q.data()); + q_.data = Eigen::Map>(q.data()); jacobian_solver_->JntToJac(q_, jacobian_); return jacobian_; } const KDL::Frame &Kinematics::compute_fk(const_jnt_array_t_ref q) { - q_.data = - Eigen::Map>(q.data()); + q_.data = Eigen::Map>(q.data()); fk_solver_->JntToCart(q_, chain_tip_frame_); return chain_tip_frame_; } const KDL::Frame &Kinematics::compute_fk(const std::vector &q) { - if (q.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (q.size() != N_JNTS) { std::string err = "Invalid number of joint positions."; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } - q_.data = - Eigen::Map>(q.data()); + q_.data = Eigen::Map>(q.data()); fk_solver_->JntToCart(q_, chain_tip_frame_); return chain_tip_frame_; } diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp index ddbbc9aa..bd8b157c 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp @@ -46,8 +46,7 @@ class TwistImpl { lbr_fri_ros2::jnt_array_t q_; std::unique_ptr kinematics_ptr_; - Eigen::Matrix - jacobian_inv_; + Eigen::Matrix jacobian_inv_; Eigen::Matrix twist_target_; }; diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp index 1d4eb80a..f7b61c29 100644 --- a/lbr_ros2_control/src/controllers/admittance_controller.cpp +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -50,12 +50,12 @@ bool AdmittanceController::reference_command_interfaces_() { joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } } @@ -77,15 +77,15 @@ void AdmittanceController::clear_state_interfaces_() { } void AdmittanceController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_names_.size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } diff --git a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp index 380827ec..232acf93 100644 --- a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp @@ -49,7 +49,7 @@ LBRJointPositionCommandController::update(const rclcpp::Time & /*time*/, } std::for_each(command_interfaces_.begin(), command_interfaces_.end(), [lbr_joint_position_command, idx = 0](auto &command_interface) mutable { - command_interface.set_value((*lbr_joint_position_command)->joint_position[idx++]); + command_interface.set_value((*lbr_joint_position_command)->joint_position[++idx]); }); return controller_interface::return_type::OK; } @@ -70,15 +70,15 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact } void LBRJointPositionCommandController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_names_.size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } diff --git a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp index b8d9f733..765bdd5b 100644 --- a/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -151,15 +151,15 @@ void LBRStateBroadcaster::init_state_msg_() { } void LBRStateBroadcaster::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_names_.size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } diff --git a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp index 22a341d2..b56bd062 100644 --- a/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp @@ -46,7 +46,7 @@ LBRTorqueCommandController::update(const rclcpp::Time & /*time*/, if (!lbr_torque_command || !(*lbr_torque_command)) { return controller_interface::return_type::OK; } - for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) { + for (std::size_t idx = 0; idx < lbr_fri_ros2::N_JNTS; ++idx) { joint_position_command_interfaces_[idx].get().set_value( (*lbr_torque_command)->joint_position[idx]); torque_command_interfaces_[idx].get().set_value((*lbr_torque_command)->torque[idx]); @@ -82,19 +82,19 @@ bool LBRTorqueCommandController::reference_command_interfaces_() { torque_command_interfaces_.emplace_back(std::ref(command_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } - if (torque_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (torque_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR(this->get_node()->get_logger(), "Number of torque command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - torque_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + torque_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } return true; @@ -106,15 +106,15 @@ void LBRTorqueCommandController::clear_command_interfaces_() { } void LBRTorqueCommandController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_names_.size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } diff --git a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp index 59763270..37a278d9 100644 --- a/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp @@ -51,7 +51,7 @@ LBRWrenchCommandController::update(const rclcpp::Time & /*time*/, if (!lbr_wrench_command || !(*lbr_wrench_command)) { return controller_interface::return_type::OK; } - for (std::size_t idx = 0; idx < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; ++idx) { + for (std::size_t idx = 0; idx < lbr_fri_ros2::N_JNTS; ++idx) { joint_position_command_interfaces_[idx].get().set_value( (*lbr_wrench_command)->joint_position[idx]); } @@ -89,12 +89,12 @@ bool LBRWrenchCommandController::reference_command_interfaces_() { wrench_command_interfaces_.emplace_back(std::ref(command_interface)); } } - if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_command_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position command interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_command_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } if (wrench_command_interfaces_.size() != CARTESIAN_DOF) { @@ -112,15 +112,15 @@ void LBRWrenchCommandController::clear_command_interfaces_() { } void LBRWrenchCommandController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_names_.size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index c259ac2e..fa827b8e 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -30,7 +30,7 @@ void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); // compute target joint veloctiy and map it to dq - Eigen::Map>(dq.data()) = + Eigen::Map>(dq.data()) = jacobian_inv_ * twist_target_; } @@ -164,12 +164,12 @@ bool TwistController::reference_state_interfaces_() { std::ref(state_interface)); } } - if (joint_position_state_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_position_state_interfaces_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint position state interfaces '%ld' does not match the number of joints " "in the robot '%d'.", - joint_position_state_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_position_state_interfaces_.size(), lbr_fri_ros2::N_JNTS); return false; } return true; @@ -183,15 +183,15 @@ void TwistController::reset_command_buffer_() { }; void TwistController::configure_joint_names_() { - if (joint_names_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (joint_names_.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR( this->get_node()->get_logger(), "Number of joint names (%ld) does not match the number of joints in the robot (%d).", - joint_names_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + joint_names_.size(), lbr_fri_ros2::N_JNTS); throw std::runtime_error("Failed to configure joint names."); } std::string robot_name = this->get_node()->get_parameter("robot_name").as_string(); - for (int i = 0; i < KUKA::FRI::LBRState::NUMBER_OF_JOINTS; i++) { + for (int i = 0; i < lbr_fri_ros2::N_JNTS; ++i) { joint_names_[i] = robot_name + "_A" + std::to_string(i + 1); } } diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index e4fdbf6c..743f0061 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -438,12 +438,11 @@ void SystemInterface::nan_state_interfaces_() { } bool SystemInterface::verify_number_of_joints_() { - if (info_.joints.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + if (info_.joints.size() != lbr_fri_ros2::N_JNTS) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME), lbr_fri_ros2::ColorScheme::ERROR - << "Expected '" << KUKA::FRI::LBRState::NUMBER_OF_JOINTS - << "' joints in URDF, got '" << info_.joints.size() << "'" - << lbr_fri_ros2::ColorScheme::ENDC); + << "Expected '" << lbr_fri_ros2::N_JNTS << "' joints in URDF, got '" + << info_.joints.size() << "'" << lbr_fri_ros2::ColorScheme::ENDC); return false; } return true;