From 5075b7da8f461b1abc9c90021d4cf2428a5d1497 Mon Sep 17 00:00:00 2001 From: mhubii Date: Thu, 7 Dec 2023 17:04:48 +0000 Subject: [PATCH] updated log --- .../include/lbr_fri_ros2/command_guard.hpp | 10 +++---- lbr_fri_ros2/src/async_client.cpp | 2 +- lbr_fri_ros2/src/command_guard.cpp | 29 +++++++++---------- 3 files changed, 20 insertions(+), 21 deletions(-) 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 79f39f65..37b4f1fd 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -20,11 +20,11 @@ struct CommandGuardParameters { using joint_array_t = std::array; using joint_name_array_t = std::array; - joint_name_array_t joint_names_; /**< Joint names.*/ - joint_array_t min_position_; /**< Minimum joint position [rad].*/ - joint_array_t max_position_; /**< Maximum joint position [rad].*/ - joint_array_t max_velocity_; /**< Maximum joint velocity [rad/s].*/ - joint_array_t max_torque_; /**< Maximum joint torque [Nm].*/ + joint_name_array_t joint_names; /**< Joint names.*/ + joint_array_t min_position; /**< Minimum joint position [rad].*/ + joint_array_t max_position; /**< Maximum joint position [rad].*/ + joint_array_t max_velocity; /**< Maximum joint velocity [rad/s].*/ + joint_array_t max_torque; /**< Maximum joint torque [Nm].*/ }; class CommandGuard { diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 66a5fff2..68a16492 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -13,7 +13,7 @@ AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters, command_interface_.log_info(); state_interface_.log_info(); RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Open loop: %s.", open_loop_ ? "true" : "false"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "AsyncClient configured."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Client configured."); } void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 88ac4ae7..d9cd835c 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -36,22 +36,23 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, void CommandGuard::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); - for (std::size_t i = 0; i < parameters_.joint_names_.size(); ++i) { + for (std::size_t i = 0; i < parameters_.joint_names.size(); ++i) { RCLCPP_INFO( rclcpp::get_logger(LOGGER_NAME), "* Joint %s limits: Position: [%.1f, %.1f] deg, velocity: %.1f deg/s, torque: %.1f Nm", - parameters_.joint_names_[i].c_str(), parameters_.min_position_[i], - parameters_.max_position_[i], parameters_.max_velocity_[i], parameters_.max_torque_[i]); + parameters_.joint_names[i].c_str(), parameters_.min_position[i] * (180. / M_PI), + parameters_.max_position[i] * (180. / M_PI), parameters_.max_velocity[i] * (180. / M_PI), + parameters_.max_torque[i]); } } bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_command, const_fri_state_t_ref /*lbr_state*/) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { - if (lbr_command.joint_position[i] < parameters_.min_position_[i] || - lbr_command.joint_position[i] > parameters_.max_position_[i]) { + if (lbr_command.joint_position[i] < parameters_.min_position[i] || + lbr_command.joint_position[i] > parameters_.max_position[i]) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Position command not in limits for joint %s.", - parameters_.joint_names_[i].c_str()); + parameters_.joint_names[i].c_str()); return false; } } @@ -63,9 +64,9 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma const double &dt = lbr_state.getSampleTime(); for (std::size_t i = 0; i < lbr_command.joint_position[i]; ++i) { if (std::abs(lbr_command.joint_position[i] - lbr_state.getMeasuredJointPosition()[i]) / dt > - parameters_.max_velocity_[i]) { + parameters_.max_velocity[i]) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint %s.", - parameters_.joint_names_[i].c_str()); + parameters_.joint_names[i].c_str()); return false; } } @@ -76,9 +77,9 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command const_fri_state_t_ref lbr_state) const { for (std::size_t i = 0; i < lbr_command.torque.size(); ++i) { if (std::abs(lbr_command.torque[i] + lbr_state.getExternalTorque()[i]) > - parameters_.max_torque_[i]) { + parameters_.max_torque[i]) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Torque command not in limits for joint %s.", - parameters_.joint_names_[i].c_str()); + parameters_.joint_names[i].c_str()); return false; } } @@ -89,13 +90,11 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l const_fri_state_t_ref lbr_state) const { for (std::size_t i = 0; i < lbr_command.joint_position.size(); ++i) { if (lbr_command.joint_position[i] < - parameters_.min_position_[i] + - parameters_.max_velocity_[i] * lbr_state.getSampleTime() || + parameters_.min_position[i] + parameters_.max_velocity[i] * lbr_state.getSampleTime() || lbr_command.joint_position[i] > - parameters_.max_position_[i] - - parameters_.max_velocity_[i] * lbr_state.getSampleTime()) { + parameters_.max_position[i] - parameters_.max_velocity[i] * lbr_state.getSampleTime()) { RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Position command not in limits for joint %s.", - parameters_.joint_names_[i].c_str()); + parameters_.joint_names[i].c_str()); return false; } }