diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index f8bb7aab..35cc1457 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -17,7 +17,7 @@ namespace lbr_fri_ros2 { class App { protected: - static constexpr char APP_LOGGER_NAME[] = "lbr_fri_ros2::App"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App"; public: App(const std::shared_ptr client_ptr); 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 f75a3d84..568b5f9c 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -17,7 +17,7 @@ namespace lbr_fri_ros2 { class AsyncClient : public KUKA::FRI::LBRClient { protected: - static constexpr char ASYNC_CLIENT_LOGGER_NAME[] = "lbr_fri_ros2::AsyncClient"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::AsyncClient"; public: AsyncClient() = delete; 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 8e5da499..79f39f65 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -29,7 +29,7 @@ struct CommandGuardParameters { class CommandGuard { protected: - static constexpr char COMMAND_GUARD_LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandGuard"; // ROS IDL types using idl_command_t = lbr_fri_msgs::msg::LBRCommand; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp index 566a1d88..002129c8 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp @@ -18,7 +18,7 @@ namespace lbr_fri_ros2 { class CommandInterface { protected: - static constexpr char COMMAND_INTERFACE_LOGGER_NAME[] = "lbr_fri_ros2::CommandInterface"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::CommandInterface"; // ROS IDL types using idl_command_t = lbr_fri_msgs::msg::LBRCommand; diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp index 555c512f..04211c95 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp @@ -19,7 +19,7 @@ struct StateInterfaceParameters { class StateInterface { protected: - static constexpr char STATE_INTERFACE_LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; + static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::StateInterface"; // ROS IDL types using idl_state_t = lbr_fri_msgs::msg::LBRState; diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 7635b67a..f13b22c4 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -16,98 +16,97 @@ App::~App() { bool App::open_udp_socket(const int &port_id, const char *const remote_host) { if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return false; } - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Opening UDP socket with port_id %d.", port_id); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Opening UDP socket with port_id %d.", port_id); if (!valid_port_(port_id)) { return false; } if (connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Socket already open."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already open."); return true; } if (!connection_ptr_->open(port_id, remote_host)) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Failed to open socket."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Failed to open socket."); return false; } - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Socket opened successfully."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket opened successfully."); return true; } bool App::close_udp_socket() { if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return false; } - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Closing UDP socket."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Closing UDP socket."); if (!connection_ptr_->isOpen()) { - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Socket already closed."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed."); return true; } connection_ptr_->close(); - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Socket closed successfully."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket closed successfully."); return true; } void App::run(int rt_prio) { if (!client_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "AsyncClient not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "AsyncClient not configured."); return; } if (!connection_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Connection not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not configured."); return; } if (!connection_ptr_->isOpen()) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Connection not open."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Connection not open."); return; } if (!app_ptr_) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "App not configured."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "App not configured."); return; } if (running_) { - RCLCPP_WARN(rclcpp::get_logger(APP_LOGGER_NAME), "App already running."); + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "App already running."); return; } run_thread_ = std::thread([&]() { if (realtime_tools::has_realtime_kernel()) { if (!realtime_tools::configure_sched_fifo(rt_prio)) { - RCLCPP_WARN(rclcpp::get_logger(APP_LOGGER_NAME), + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "Failed to set FIFO realtime scheduling policy."); } } else { - RCLCPP_WARN(rclcpp::get_logger(APP_LOGGER_NAME), - "Realtime kernel recommended but not required."); + RCLCPP_WARN(rclcpp::get_logger(LOGGER_NAME), "Realtime kernel recommended but not required."); } - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Starting run thread."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Starting run thread."); should_stop_ = false; running_ = true; bool success = true; while (rclcpp::ok() && success && !should_stop_) { success = app_ptr_->step(); // TODO: blocks until robot heartbeat, stuck if port id mismatches if (client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) { - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "LBR in session state idle, exiting."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting."); break; } } client_ptr_->get_state_interface().uninitialize(); running_ = false; - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Exiting run thread."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Exiting run thread."); }); run_thread_.detach(); } void App::stop_run() { - RCLCPP_INFO(rclcpp::get_logger(APP_LOGGER_NAME), "Requesting run thread stop."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Requesting run thread stop."); should_stop_ = true; } bool App::valid_port_(const int &port_id) { if (port_id < 30200 || port_id > 30209) { - RCLCPP_ERROR(rclcpp::get_logger(APP_LOGGER_NAME), "Expected port_id in [30200, 30209], got %d.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Expected port_id in [30200, 30209], got %d.", port_id); return false; } diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 8d994c7a..66a5fff2 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -7,19 +7,18 @@ AsyncClient::AsyncClient(const CommandGuardParameters &command_guard_parameters, const bool &open_loop) : command_interface_(command_guard_parameters, command_guard_variant), state_interface_(state_interface_parameters), open_loop_(open_loop) { - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "Configuring client."); - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "Command guard variant: %s.", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Configuring client."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Command guard variant: %s.", command_guard_variant.c_str()); command_interface_.log_info(); state_interface_.log_info(); - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "Open loop: %s.", - open_loop_ ? "true" : "false"); - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "AsyncClient configured."); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Open loop: %s.", open_loop_ ? "true" : "false"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "AsyncClient configured."); } void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, KUKA::FRI::ESessionState new_state) { - RCLCPP_INFO(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), "LBR switched from %s to %s.", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR switched from %s to %s.", EnumMaps::session_state_map(old_state).c_str(), EnumMaps::session_state_map(new_state).c_str()); command_interface_.init_command(robotState()); @@ -61,7 +60,7 @@ void AsyncClient::command() { default: std::string err = "Unsupported command mode: " + std::to_string(robotState().getClientCommandMode()) + "."; - RCLCPP_ERROR(rclcpp::get_logger(ASYNC_CLIENT_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } } diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 2693ebbf..a98e22ae 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -28,18 +28,17 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command, } return true; default: - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Invalid EClientCommandMode provided."); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Invalid EClientCommandMode provided."); return false; } return false; } void CommandGuard::log_info() const { - RCLCPP_INFO(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), "Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Parameters:"); for (std::size_t i = 0; i < parameters_.joint_names_.size(); ++i) { RCLCPP_INFO( - rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), + 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]); @@ -51,8 +50,7 @@ bool CommandGuard::command_in_position_limits_(const_idl_command_t_ref lbr_comma 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]) { - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Position command not in limits for joint %s.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Position command not in limits for joint %s.", parameters_.joint_names_[i].c_str()); return false; } @@ -66,8 +64,8 @@ bool CommandGuard::command_in_velocity_limits_(const_idl_command_t_ref lbr_comma 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]) { - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Velocity not in limits for joint %s.", parameters_.joint_names_[i].c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Velocity not in limits for joint %s.", + parameters_.joint_names_[i].c_str()); return false; } } @@ -79,8 +77,7 @@ bool CommandGuard::command_in_torque_limits_(const_idl_command_t_ref lbr_command 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]) { - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Torque command not in limits for joint %s.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Torque command not in limits for joint %s.", parameters_.joint_names_[i].c_str()); return false; } @@ -97,8 +94,7 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l lbr_command.joint_position[i] > parameters_.max_position_[i] - parameters_.max_velocity_[i] * lbr_state.getSampleTime()) { - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_GUARD_LOGGER_NAME), - "Position command not in limits for joint %s.", + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), "Position command not in limits for joint %s.", parameters_.joint_names_[i].c_str()); return false; } @@ -109,7 +105,7 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l std::unique_ptr command_guard_factory(const CommandGuardParameters &command_guard_parameters, const std::string &variant) { - constexpr char COMMANG_GUARD_FACTORY_LOGGER_NAME[] = "lbr_fri_ros2::command_guard_factory"; + constexpr char LOGGER_NAME[] = "lbr_fri_ros2::command_guard_factory"; if (variant == "default") { return std::make_unique(command_guard_parameters); } @@ -117,7 +113,7 @@ command_guard_factory(const CommandGuardParameters &command_guard_parameters, return std::make_unique(command_guard_parameters); } std::string err = "Invalid CommandGuard variant provided."; - RCLCPP_ERROR(rclcpp::get_logger(COMMANG_GUARD_FACTORY_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/command_interface.cpp b/lbr_fri_ros2/src/command_interface.cpp index e95379ef..25677bc1 100644 --- a/lbr_fri_ros2/src/command_interface.cpp +++ b/lbr_fri_ros2/src/command_interface.cpp @@ -4,7 +4,7 @@ namespace lbr_fri_ros2 { CommandInterface::CommandInterface(const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) { - RCLCPP_INFO(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Configuring command interface with command guard '%s'.", command_guard_variant.c_str()); command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); @@ -14,12 +14,12 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) { std::string err = "Set joint position only allowed in position command mode."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } if (!command_guard_) { std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -35,7 +35,7 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -46,12 +46,12 @@ void CommandInterface::get_joint_position_command(fri_command_t_ref command, void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::TORQUE) { std::string err = "Set torque only allowed in torque command mode."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } if (!command_guard_) { std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -78,12 +78,12 @@ void CommandInterface::get_torque_command(fri_command_t_ref command, const_fri_s void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_state_t_ref state) { if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::WRENCH) { std::string err = "Set wrench only allowed in wrench command mode."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } if (!command_guard_) { std::string err = "Uninitialized command guard."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } @@ -100,7 +100,7 @@ void CommandInterface::get_wrench_command(fri_command_t_ref command, const_fri_s // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; - RCLCPP_ERROR(rclcpp::get_logger(COMMAND_INTERFACE_LOGGER_NAME), err.c_str()); + RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); throw std::runtime_error(err); } diff --git a/lbr_fri_ros2/src/state_interface.cpp b/lbr_fri_ros2/src/state_interface.cpp index 3eb97933..06f25a73 100644 --- a/lbr_fri_ros2/src/state_interface.cpp +++ b/lbr_fri_ros2/src/state_interface.cpp @@ -81,12 +81,10 @@ void StateInterface::init_filters_() { } void StateInterface::log_info() const { - RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), "Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), - " external_torque_cutoff_frequency: %f", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), " external_torque_cutoff_frequency: %f", parameters_.external_torque_cutoff_frequency); - RCLCPP_INFO(rclcpp::get_logger(STATE_INTERFACE_LOGGER_NAME), - " measured_torque_cutoff_frequency: %f", + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), " measured_torque_cutoff_frequency: %f", parameters_.measured_torque_cutoff_frequency); } } // namespace lbr_fri_ros2