Skip to content

Commit

Permalink
simplified logger names
Browse files Browse the repository at this point in the history
  • Loading branch information
@mhubii
mhubii committed Dec 7, 2023
1 parent f337b4e commit 4bc0173
Show file tree
Hide file tree
Showing 10 changed files with 54 additions and 62 deletions.
2 changes: 1 addition & 1 deletion lbr_fri_ros2/include/lbr_fri_ros2/app.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<AsyncClient> client_ptr);
Expand Down
2 changes: 1 addition & 1 deletion lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
43 changes: 21 additions & 22 deletions lbr_fri_ros2/src/app.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
13 changes: 6 additions & 7 deletions lbr_fri_ros2/src/async_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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);
}
}
Expand Down
24 changes: 10 additions & 14 deletions lbr_fri_ros2/src/command_guard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]);
Expand All @@ -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;
}
Expand All @@ -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;
}
}
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -109,15 +105,15 @@ bool SafeStopCommandGuard::command_in_position_limits_(const_idl_command_t_ref l
std::unique_ptr<CommandGuard>
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<CommandGuard>(command_guard_parameters);
}
if (variant == "safe_stop") {
return std::make_unique<SafeStopCommandGuard>(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
18 changes: 9 additions & 9 deletions lbr_fri_ros2/src/command_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand All @@ -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);
}

Expand Down
8 changes: 3 additions & 5 deletions lbr_fri_ros2/src/state_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

0 comments on commit 4bc0173

Please sign in to comment.