From f48045f0a79cc9ca580b7b736d0a0da278365af1 Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Wed, 19 Nov 2025 17:15:06 +0100 Subject: [PATCH 1/3] reconstruct logger with spdlog & cartesian_pose_controller --- CMakeLists.txt | 2 + .../control_mode/abstract_control_mode.hpp | 22 ++--- include/control_mode/control_mode_factory.hpp | 3 +- include/franka_gripper_proxy.hpp | 21 ++-- include/utils/franka_config.hpp | 8 +- include/utils/logger.hpp | 95 +++++-------------- include/utils/service_registry.hpp | 22 ++--- src/control_mode/cartesian_pose_mode.cpp | 62 ++++++------ src/control_mode/cartesian_velocity_mode.cpp | 17 ++-- src/control_mode/human_control_mode.cpp | 20 ++-- src/control_mode/idle_control_mode.cpp | 19 ++-- src/control_mode/joint_position_mode.cpp | 16 ++-- src/control_mode/joint_velocity_mode.cpp | 16 ++-- src/franka_arm_proxy.cpp | 28 +++--- src/main.cpp | 33 ++++++- 15 files changed, 188 insertions(+), 196 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6d56973..6adc6a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ find_package(Threads REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(ZMQ REQUIRED libzmq) find_package(yaml-cpp REQUIRED) +find_package(spdlog REQUIRED) # ----------------------------- # Source collection @@ -71,4 +72,5 @@ target_link_libraries(proxy ${ZMQ_LIBRARIES} ${YAML_CPP_LIBRARIES} Threads::Threads + spdlog::spdlog ) diff --git a/include/control_mode/abstract_control_mode.hpp b/include/control_mode/abstract_control_mode.hpp index 51e40f5..d8a3ef3 100644 --- a/include/control_mode/abstract_control_mode.hpp +++ b/include/control_mode/abstract_control_mode.hpp @@ -6,9 +6,9 @@ #include #include #include -#include #include #include +#include #include "utils/atomic_double_buffer.hpp" #include "utils/zmq_context.hpp" @@ -25,34 +25,34 @@ class AbstractControlMode { virtual void start() { startRobot(); control_thread_ = std::thread(&AbstractControlMode::controlLoop, this); - std::cout << "[" << getModeName() << "] Control thread launched.\n"; + spdlog::info("[{}] Control thread launched.", getModeName()); command_thread_ = std::thread(&AbstractControlMode::commandSubscriptionLoop, this); - std::cout << "[" << getModeName() << "] Command subscription thread launched.\n"; + spdlog::info("[{}] Command subscription thread launched.", getModeName()); }; void startRobot() { #if !LOCAL_TESTING if (!robot_ || !model_) { - std::cerr << "[ " << getModeName() << "] Robot or model not set.\n"; + spdlog::error("[{}] Robot or model not set.", getModeName()); return; } robot_->automaticErrorRecovery(); #endif - std::cout << "[" << getModeName() << "] Robot control started.\n"; + spdlog::info("[{}] Robot control started.", getModeName()); is_running_ = true; }; virtual void stop() { is_running_ = false; if (control_thread_.joinable()) { - std::cout << "[" << getModeName() << "] Stopping control thread...\n"; + spdlog::info("[{}] Stopping control thread...", getModeName()); control_thread_.join(); } if (command_thread_.joinable()) { - std::cout << "[" << getModeName() << "] Stopping command subscription thread...\n"; + spdlog::info("[{}] Stopping command subscription thread...", getModeName()); command_thread_.join(); } - std::cout << "[" << getModeName() << "] Stopped.\n"; + spdlog::info("[{}] Stopped.", getModeName()); }; // Get the mode ID for this control mode virtual protocol::ModeID getModeID() const = 0; // Return the mode ID as an integer @@ -96,7 +96,7 @@ class AbstractControlMode { zmq::socket_t sub_socket_(ZmqContext::instance(), ZMQ_SUB); sub_socket_.set(zmq::sockopt::rcvtimeo, 1000); // 1 second timeout if (command_sub_addr_.empty()) { - std::cerr << "[" << getModeName() << "] Command subscription address is empty. Exiting command subscription loop." << std::endl; + spdlog::warn("[{}] Command subscription address is empty. Exiting command subscription loop.", getModeName()); return; } sub_socket_.connect(command_sub_addr_); @@ -115,12 +115,12 @@ class AbstractControlMode { }; writeCommand(data); } catch (const zmq::error_t& e) { - std::cerr << "[FrankaProxy] ZMQ recv error: " << e.what() << std::endl; + spdlog::error("[FrankaProxy] ZMQ recv error: {}", e.what()); break; } } sub_socket_.close(); - std::cout << "[" << getModeName() << "] Command subscription loop exited." << std::endl; + spdlog::info("[{}] Command subscription loop exited.", getModeName()); }; virtual void writeCommand(const protocol::ByteView& data) = 0; diff --git a/include/control_mode/control_mode_factory.hpp b/include/control_mode/control_mode_factory.hpp index fab7da4..8b4503b 100644 --- a/include/control_mode/control_mode_factory.hpp +++ b/include/control_mode/control_mode_factory.hpp @@ -4,13 +4,14 @@ #include #include #include +#include class ControlModeFactory { public: static void registerMode(const std::string& name, std::function()> creator) { - std::cout << "[ControlModeFactory] Registering mode: " << name << std::endl; + spdlog::info("[ControlModeFactory] Registering mode: {}", name); getRegistry()[name] = std::move(creator); } diff --git a/include/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp index 356da1e..19bdb54 100644 --- a/include/franka_gripper_proxy.hpp +++ b/include/franka_gripper_proxy.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -49,7 +50,7 @@ class FrankaGripperProxy { // Core server operations void start() { is_running = true; - std::cout << is_running <<"gripper control"<< std::endl; + spdlog::info("Gripper proxy running flag set to {}", is_running.load()); // state_pub_thread_ = std::thread(&FrankaGripperProxy::statePublishThread, this); service_registry_.start(); state_pub_thread_ = std::thread(&FrankaGripperProxy::statePubThread, this); @@ -57,7 +58,7 @@ class FrankaGripperProxy { }; void stop() { - std::cout << "[INFO] Stopping FrankaGripperProxy..." << std::endl; + spdlog::info("Stopping FrankaGripperProxy..."); is_running = false; if (state_pub_thread_.joinable()) state_pub_thread_.join(); // try close ZeroMQ sockets @@ -65,7 +66,7 @@ class FrankaGripperProxy { // wait for closing service_registry_.stop(); gripper_.reset(); - std::cout << "[INFO] FrankaGripperProxy stopped successfully." << std::endl; + spdlog::info("FrankaGripperProxy stopped successfully."); }; private: @@ -109,7 +110,7 @@ class FrankaGripperProxy { } catch(const std::exception& e) { - std::cerr << e.what() << '\n'; + spdlog::error("{}", e.what()); } } }; @@ -130,7 +131,7 @@ class FrankaGripperProxy { try { state_pub_socket_.close(); } catch (const zmq::error_t& e) { - std::cerr << "[ZMQ ERROR] " << e.what() << "\n"; + spdlog::error("[ZMQ ERROR] {}", e.what()); } }; @@ -149,7 +150,7 @@ class FrankaGripperProxy { } catch(const std::exception& e) { - std::cerr << e.what() << '\n'; + spdlog::error("{}", e.what()); } #endif std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -174,9 +175,9 @@ class FrankaGripperProxy { }; command_.write(protocol::decode(data)); // std::this_thread::sleep_for(std::chrono::milliseconds(10)); - std::cout << "[FrankaGripperProxy] Received new gripper command: width=" - << command_.read().width << ", speed=" << command_.read().speed - << ", force=" << command_.read().force << std::endl; + const auto cmd = command_.read(); + spdlog::info("[FrankaGripperProxy] Received new gripper command: width={}, speed={}, force={}", + cmd.width, cmd.speed, cmd.force); } }; @@ -192,7 +193,7 @@ class FrankaGripperProxy { ServiceRegistry service_registry_; void startFrankaGripperControl(const std::string& command_sub_addr) { if (is_on_control_mode) { - std::cout << "[FrankaGripperProxy] Already in control mode, now quit previous control mode." << std::endl; + spdlog::warn("[FrankaGripperProxy] Already in control mode, stopping previous command subscriber."); is_on_control_mode = false; if (command_sub_thread_.joinable()) command_sub_thread_.join(); } diff --git a/include/utils/franka_config.hpp b/include/utils/franka_config.hpp index d868ae3..d020a63 100644 --- a/include/utils/franka_config.hpp +++ b/include/utils/franka_config.hpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -16,7 +16,7 @@ class FrankaConfig { void loadFromFile(const std::string& file_path) { std::ifstream file(file_path); if (!file.is_open()) { - std::cerr << "Error: Could not open config file: " << file_path << std::endl; + spdlog::error("Error: Could not open config file: {}", file_path); return; } @@ -39,7 +39,7 @@ class FrankaConfig { if (it != config_data.end()) { return it->second; } else { - std::cerr << "Warning: Key not found in config: " << key << ", using default: " << default_value << std::endl; + spdlog::warn("Warning: Key not found in config: {}, using default: {}", key, default_value); return default_value; } } @@ -48,7 +48,7 @@ class FrankaConfig { // Function to display the loaded configuration void display() const { for (const auto& pair : config_data) { - std::cout << pair.first << ": " << pair.second << std::endl; + spdlog::info("{}: {}", pair.first, pair.second); } } diff --git a/include/utils/logger.hpp b/include/utils/logger.hpp index 8823c94..3bd2bf6 100644 --- a/include/utils/logger.hpp +++ b/include/utils/logger.hpp @@ -1,12 +1,11 @@ #pragma once -#include -#include -#include +#include #include -#include -#include +#include #include -#include + +#include +#include namespace utils { @@ -31,31 +30,28 @@ class Logger { void setOutputFile(const std::string& path) { std::lock_guard lock(mutex_); - if (file_.is_open()) file_.close(); - file_.open(path, std::ios::out | std::ios::app); + file_logger_ = spdlog::basic_logger_mt("utils_file_logger", path, true); } - void enableColor(bool enable = true) { - use_color_ = enable; + void enableColor(bool /*enable*/ = true) { + // color formatting handled by spdlog's default logger pattern } template void log(LogLevel level, Args&&... args) { - if (level < level_) return; // filtered out + if (level < level_) return; std::ostringstream oss; - (oss << ... << args); // fold expression (C++17) - - std::lock_guard lock(mutex_); - std::string output = format(level, oss.str()); + (oss << ... << args); - if (file_.is_open()) - file_ << output << std::endl; - else - std::cout << output << std::endl; + const auto spd_level = toSpdLevel(level); + if (file_logger_) { + file_logger_->log(spd_level, oss.str()); + } else { + spdlog::log(spd_level, oss.str()); + } } - // Helper macros for convenience template void trace(Args&&... a) { log(LogLevel::TRACE, std::forward(a)...); } template void info(Args&&... a) { log(LogLevel::INFO, std::forward(a)...); } template void warn(Args&&... a) { log(LogLevel::WARN, std::forward(a)...); } @@ -64,66 +60,25 @@ class Logger { private: Logger() = default; - ~Logger() { if (file_.is_open()) file_.close(); } - - std::string format(LogLevel level, const std::string& msg) { - // Timestamp - auto now = std::chrono::system_clock::now(); - auto t = std::chrono::system_clock::to_time_t(now); - std::tm tm; - #ifdef _WIN32 - localtime_s(&tm, &t); - #else - localtime_r(&t, &tm); - #endif - - std::ostringstream oss; - oss << std::put_time(&tm, "%H:%M:%S"); - - // Thread ID - oss << " [" << std::this_thread::get_id() << "] "; - - // Level - if (use_color_) - oss << levelColor(level) << levelName(level) << "\033[0m"; - else - oss << levelName(level); - - oss << " : " << msg; - return oss.str(); - } - - std::string levelName(LogLevel level) const { - switch (level) { - case LogLevel::TRACE: return "TRACE"; - case LogLevel::INFO: return "INFO "; - case LogLevel::WARN: return "WARN "; - case LogLevel::ERROR: return "ERROR"; - case LogLevel::FATAL: return "FATAL"; - } - return "UNKWN"; - } - std::string levelColor(LogLevel level) const { + static spdlog::level::level_enum toSpdLevel(LogLevel level) { switch (level) { - case LogLevel::TRACE: return "\033[37m"; // white - case LogLevel::INFO: return "\033[36m"; // cyan - case LogLevel::WARN: return "\033[33m"; // yellow - case LogLevel::ERROR: return "\033[31m"; // red - case LogLevel::FATAL: return "\033[41m"; // red background + case LogLevel::TRACE: return spdlog::level::trace; + case LogLevel::INFO: return spdlog::level::info; + case LogLevel::WARN: return spdlog::level::warn; + case LogLevel::ERROR: return spdlog::level::err; + case LogLevel::FATAL: return spdlog::level::critical; } - return ""; + return spdlog::level::info; } LogLevel level_ = LogLevel::TRACE; - std::ofstream file_; - bool use_color_ = true; - mutable std::mutex mutex_; + std::shared_ptr file_logger_; + std::mutex mutex_; }; } // namespace utils -// Convenience macros #define LOG_TRACE(...) utils::Logger::instance().trace(__VA_ARGS__) #define LOG_INFO(...) utils::Logger::instance().info(__VA_ARGS__) #define LOG_WARN(...) utils::Logger::instance().warn(__VA_ARGS__) diff --git a/include/utils/service_registry.hpp b/include/utils/service_registry.hpp index 2535db5..2f8ed5e 100644 --- a/include/utils/service_registry.hpp +++ b/include/utils/service_registry.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include "protocol/codec.hpp" #include "protocol/msg_header.hpp" #include "protocol/msg_id.hpp" @@ -88,7 +88,7 @@ class ServiceRegistry { void handleRequest(const std::string& service_name, const protocol::ByteView& payload, protocol::FrankaResponse& response) { auto it = handlers_.find(service_name); - std::cout << "[ServiceRegistry] Handling message of type " << service_name << std::endl; + spdlog::info("[ServiceRegistry] Handling message of type {}", service_name); if (it == handlers_.end()) { // const std::string err = "Unknown handler"; // protocol::FrankaResponse rr(protocol::RequestResultCode::FAIL, err); @@ -103,10 +103,10 @@ class ServiceRegistry { try { response.payload = it->second(payload); } catch (const std::exception& e) { - std::cerr << "[ServiceRegistry] Exception while handling " << service_name << " service request: " << e.what() << std::endl; + spdlog::error("[ServiceRegistry] Exception while handling {} service request: {}", service_name, e.what()); response.code = protocol::FrankaResponseCode::FAIL; } - std::cout << "[ServiceRegistry] Found handler for service " << service_name << std::endl; + spdlog::info("[ServiceRegistry] Found handler for service {}", service_name); } void clearHandlers() { @@ -122,7 +122,7 @@ class ServiceRegistry { while (is_running) { zmq::message_t service_name_msg; if (!res_socket_.recv(service_name_msg, zmq::recv_flags::none)) continue; - std::cout << "[FrankaArmProxy] Received service request message of size " << service_name_msg.size() << " bytes." << std::endl; + spdlog::debug("[FrankaArmProxy] Received service request frame of size {} bytes.", service_name_msg.size()); std::string service_name = protocol::decode(protocol::ByteView{ static_cast(service_name_msg.data()), service_name_msg.size() @@ -130,27 +130,27 @@ class ServiceRegistry { if (!service_name_msg.more()) { - std::cerr << "[FrankaArmProxy] Warning: No payload frame received for service request." << std::endl; + spdlog::warn("[FrankaArmProxy] Warning: No payload frame received for service request."); continue; // Skip this iteration if no payload } zmq::message_t payload_msg; if (!res_socket_.recv(payload_msg, zmq::recv_flags::none)) continue; - std::cout << "[FrankaArmProxy] Received payload message of size " << payload_msg.size() << " bytes." << std::endl; + spdlog::debug("[FrankaArmProxy] Received payload frame of size {} bytes.", payload_msg.size()); protocol::ByteView payload{ static_cast(payload_msg.data()), payload_msg.size() }; if (payload_msg.more()) { - std::cerr << "[FrankaArmProxy] Warning: More message frames received than expected." << std::endl; + spdlog::warn("[FrankaArmProxy] Warning: More message frames received than expected."); } - std::cout << "[FrankaArmProxy] Received request " << service_name << std::endl; + spdlog::info("[FrankaArmProxy] Received request {}", service_name); //std::string response; protocol::FrankaResponse response; handleRequest(service_name, payload, response); //send response res_socket_.send(zmq::buffer(service_name), zmq::send_flags::sndmore); res_socket_.send(zmq::buffer(response.payload), zmq::send_flags::none); - std::cout << "[FrankaArmProxy] Sent response: msg size = " << response.payload.size() << std::endl; + spdlog::info("[FrankaArmProxy] Sent response payload of {} bytes.", response.payload.size()); } } @@ -168,4 +168,4 @@ class ServiceRegistry { static constexpr int SOCKET_TIMEOUT_MS = 100; std::thread service_thread_; std::string service_addr_; - }; \ No newline at end of file + }; diff --git a/src/control_mode/cartesian_pose_mode.cpp b/src/control_mode/cartesian_pose_mode.cpp index a5627f2..29e9918 100644 --- a/src/control_mode/cartesian_pose_mode.cpp +++ b/src/control_mode/cartesian_pose_mode.cpp @@ -2,66 +2,74 @@ #include "protocol/mode_id.hpp" #include "protocol/codec.hpp" #include -#include - +#include +//Note: Pose is represented as a 4x4 matrix in column-major format. CartesianPoseMode::CartesianPoseMode(): desired_pose_(franka::CartesianPose{ {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0} + 0.306, 0.0, 0.485, 1.0} }) {}; CartesianPoseMode::~CartesianPoseMode() = default; void CartesianPoseMode::controlLoop() { - std::cout << "[CartesianPoseMode] Started.\n"; + spdlog::info("[CartesianPoseMode] Started."); is_running_ = true; // Initialize desired Cartesian pose to identity (no movement)` desired_pose_.write(franka::CartesianPose{ - {1.0, 0.0, 0.0, 0.0, // Row 1 (rotation) - 0.0, 1.0, 0.0, 0.0, // Row 2 (rotation) - 0.0, 0.0, 1.0, 0.0, // Row 3 (rotation) - 0.0, 0.0, 0.0, 1.0} // Row 4 (translation, homogeneous) + {1.0, 0.0, 0.0, 0.0, // col 1 (rotationx 0) + 0.0, 1.0, 0.0, 0.0, // col 2 (rotationy 0) + 0.0, 0.0, 1.0, 0.0, // col 3 (rotationz 0) + 0.306, 0.0, 0.485} // col 4 (translation 1) }); if (!robot_ || !model_) { - std::cerr << "[CartesianPoseMode] Robot or model not set.\n"; + spdlog::error("[CartesianPoseMode] Robot or model not set."); return; } robot_->automaticErrorRecovery(); - std::function callback = + std::function motion_generator_callback = [this](const franka::RobotState& state, franka::Duration) -> franka::CartesianPose { - if (!is_running_) { - throw franka::ControlException("CartesianPoseMode stopped."); - } - + // if (!is_running_) { + // throw franka::ControlException("CartesianPoseMode stopped."); + // } updateRobotState(state); auto desired = desired_pose_.read(); - if (!is_running_) { return franka::MotionFinished(desired); } return desired; }; + bool is_robot_operational = true; + while (is_running_ && is_robot_operational) { + try { + robot_->control(motion_generator_callback); + } catch (const std::exception &ex) { + spdlog::error("[CartesianPoseMode] Robot is unable to be controlled: {}", ex.what()); + is_robot_operational = false; + } + for (int i = 0; i < 3; i++) { + spdlog::warn("[CartesianPoseMode] Waiting {} seconds before recovery attempt...", 3); - try { - robot_->control(callback); - } catch (const franka::ControlException& e) { - std::cerr << "[CartesianPoseMode] Exception: " << e.what() << std::endl; - if (std::string(e.what()).find("reflex") != std::string::npos) { - std::cout << "Reflex detected, attempting automatic recovery...\n"; - try { - robot_->automaticErrorRecovery(); - } catch (const franka::Exception& recovery_error) { - std::cerr << "Recovery failed: " << recovery_error.what() << std::endl; + // Wait + usleep(1000 * 3); + // Attempt recovery + try { + robot_->automaticErrorRecovery(); + spdlog::info("[CartesianPoseMode] Robot operation recovered."); + is_robot_operational = true; + break; + } catch (const std::exception &ex) { + spdlog::error("[CartesianPoseMode] Recovery failed: {}", ex.what()); } } - std::cout << "[CartesianPoseMode] Exited.\n"; + } } @@ -77,4 +85,4 @@ void CartesianPoseMode::writeCommand(const protocol::ByteView& data) { void CartesianPoseMode::writeZeroCommand() { franka::CartesianPose current_pose = current_state_->read().O_T_EE; desired_pose_.write(current_pose); -} \ No newline at end of file +} diff --git a/src/control_mode/cartesian_velocity_mode.cpp b/src/control_mode/cartesian_velocity_mode.cpp index c7dc1ed..c69eae5 100644 --- a/src/control_mode/cartesian_velocity_mode.cpp +++ b/src/control_mode/cartesian_velocity_mode.cpp @@ -3,7 +3,7 @@ #include "protocol/codec.hpp" #include #include -#include +#include #include CartesianVelocityMode::CartesianVelocityMode(): @@ -12,13 +12,13 @@ CartesianVelocityMode::CartesianVelocityMode(): CartesianVelocityMode::~CartesianVelocityMode() = default; void CartesianVelocityMode::controlLoop() { - std::cout << "[CartesianVelocityMode] Started.\n"; + spdlog::info("[CartesianVelocityMode] Started."); is_running_ = true; desired_velocities_.write(franka::CartesianVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); if (!robot_ || !model_) { - std::cerr << "[CartesianVelocityMode] Robot or model not set.\n"; + spdlog::error("[CartesianVelocityMode] Robot or model not set."); return; } @@ -62,12 +62,11 @@ void CartesianVelocityMode::controlLoop() { // robot_->control(impedance_control_callback, motion_generator_callback, true, 1); robot_->control(motion_generator_callback, franka::ControllerMode::kCartesianImpedance, true, 1); } catch (const std::exception &ex) { - std::cout << "Robot is unable to be controlled: " << ex.what() << std::endl; + spdlog::error("[CartesianVelocityMode] Robot is unable to be controlled: {}", ex.what()); is_robot_operational = false; } for (int i = 0; i < 3; i++) { - std::cout << "[CartesianVelocityMode] Waiting " << 3 - << " seconds before recovery attempt...\n"; + spdlog::warn("[CartesianVelocityMode] Waiting {} seconds before recovery attempt...", 3); // Wait usleep(1000 * 3); @@ -75,11 +74,11 @@ void CartesianVelocityMode::controlLoop() { // Attempt recovery try { robot_->automaticErrorRecovery(); - std::cout << "[CartesianVelocityMode] Robot operation recovered.\n"; + spdlog::info("[CartesianVelocityMode] Robot operation recovered."); is_robot_operational = true; break; } catch (const std::exception &ex) { - std::cout << "[CartesianVelocityMode] Recovery failed: " << ex.what() << std::endl; + spdlog::error("[CartesianVelocityMode] Recovery failed: {}", ex.what()); } } } @@ -97,4 +96,4 @@ void CartesianVelocityMode::writeCommand(const protocol::ByteView& data) { void CartesianVelocityMode::writeZeroCommand() { desired_velocities_.write(franka::CartesianVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); -} \ No newline at end of file +} diff --git a/src/control_mode/human_control_mode.cpp b/src/control_mode/human_control_mode.cpp index d19f96d..a911bf8 100644 --- a/src/control_mode/human_control_mode.cpp +++ b/src/control_mode/human_control_mode.cpp @@ -1,16 +1,16 @@ #include "human_control_mode.hpp" #include -#include +#include HumanControlMode::HumanControlMode() = default; HumanControlMode::~HumanControlMode() = default; void HumanControlMode::start() { is_running_ = true; - std::cout << "[HumanControlMode] Started.\n"; + spdlog::info("[HumanControlMode] Started."); if (!robot_ || !model_) { - std::cerr << "[HumanControlMode] Robot or model not set.\n"; + spdlog::error("[HumanControlMode] Robot or model not set."); return; } robot_->setCollisionBehavior( @@ -36,16 +36,16 @@ void HumanControlMode::start() { try { robot_->control(callback); } catch (const franka::ControlException& e) { - std::cerr << "[HumanControlMode] Exception: " << e.what() << std::endl; + spdlog::error("[HumanControlMode] Exception: {}", e.what()); if (std::string(e.what()).find("reflex") != std::string::npos) { - std::cout << "Reflex detected, attempting automatic recovery...\n"; + spdlog::warn("Reflex detected, attempting automatic recovery..."); try { robot_->automaticErrorRecovery(); } catch (const franka::Exception& recovery_error) { - std::cerr << "Recovery failed: " << recovery_error.what() << std::endl; + spdlog::error("Recovery failed: {}", recovery_error.what()); } } - std::cout << "[HumanControlMode] Exited.\n"; + spdlog::info("[HumanControlMode] Exited."); } } @@ -56,14 +56,14 @@ protocol::ModeID HumanControlMode::getModeID() const { void HumanControlMode::writeCommand(const protocol::ByteView& data) { // HumanControlMode does not process external commands; ignore incoming data. - std::cout << "[HumanControlMode] Received command data, but this mode does not accept commands.\n"; + spdlog::warn("[HumanControlMode] Received command data, but this mode does not accept commands."); } void HumanControlMode::controlLoop() { // HumanControlMode control logic is handled in start(); this function is unused. - std::cout << "[HumanControlMode] controlLoop() called, but control is managed in start().\n"; + spdlog::warn("[HumanControlMode] controlLoop() called, but control is managed in start()."); } void HumanControlMode::writeZeroCommand() { // No action needed for zero command in human control mode -} \ No newline at end of file +} diff --git a/src/control_mode/idle_control_mode.cpp b/src/control_mode/idle_control_mode.cpp index 8915286..feb5ccf 100644 --- a/src/control_mode/idle_control_mode.cpp +++ b/src/control_mode/idle_control_mode.cpp @@ -1,4 +1,5 @@ #include "idle_control_mode.hpp" +#include IdleControlMode::IdleControlMode() = default; @@ -10,11 +11,11 @@ protocol::ModeID IdleControlMode::getModeID() const { return protocol::ModeID::IDLE; } -void IdleControlMode::start() { - startRobot(); - control_thread_ = std::thread(&IdleControlMode::controlLoop, this); - std::cout << "[" << getModeName() << "] Control thread launched.\n"; -} + void IdleControlMode::start() { + startRobot(); + control_thread_ = std::thread(&IdleControlMode::controlLoop, this); + spdlog::info("[{}] Control thread launched.", getModeName()); + } void IdleControlMode::controlLoop() { @@ -27,7 +28,7 @@ void IdleControlMode::controlLoop() { } #else if (!robot_ || !model_) { - std::cerr << "[IdleControlMode] Robot or model not set.\n"; + spdlog::error("[IdleControlMode] Robot or model not set."); return; } while (is_running_) { @@ -37,10 +38,10 @@ void IdleControlMode::controlLoop() { current_state_->write(state); } } catch (const franka::Exception& e) { - std::cerr << "[IdleMode] readOnce() failed: " << e.what() << std::endl; + spdlog::error("[IdleMode] readOnce() failed: {}", e.what()); } } - std::cout << "[IdleControlMode] Exited.\n"; + spdlog::info("[IdleControlMode] Exited."); #endif } @@ -51,4 +52,4 @@ void IdleControlMode::writeCommand(const protocol::ByteView& data) { void IdleControlMode::writeZeroCommand() { // No action needed for zero command in idle mode -} \ No newline at end of file +} diff --git a/src/control_mode/joint_position_mode.cpp b/src/control_mode/joint_position_mode.cpp index 4ca2db3..a6ef363 100644 --- a/src/control_mode/joint_position_mode.cpp +++ b/src/control_mode/joint_position_mode.cpp @@ -1,6 +1,6 @@ #include "control_mode/joint_position_mode.hpp" #include -#include +#include #include "protocol/codec.hpp" JointPositionMode::JointPositionMode(): @@ -9,13 +9,13 @@ JointPositionMode::JointPositionMode(): JointPositionMode::~JointPositionMode() = default; void JointPositionMode::controlLoop() { - std::cout << "[JointPositionMode] Started.\n"; + spdlog::info("[JointPositionMode] Started."); is_running_ = true; desired_positions_.write(franka::JointPositions{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); if (!robot_ || !model_) { - std::cerr << "[JointPositionMode] Robot or model not set.\n"; + spdlog::error("[JointPositionMode] Robot or model not set."); return; } @@ -37,16 +37,16 @@ void JointPositionMode::controlLoop() { try { robot_->control(callback); } catch (const franka::ControlException& e) { - std::cerr << "[JointPositionMode] Exception: " << e.what() << std::endl; + spdlog::error("[JointPositionMode] Exception: {}", e.what()); if (std::string(e.what()).find("reflex") != std::string::npos) { - std::cout << "Reflex detected, attempting automatic recovery...\n"; + spdlog::warn("Reflex detected, attempting automatic recovery..."); try { robot_->automaticErrorRecovery(); } catch (const franka::Exception& recovery_error) { - std::cerr << "Recovery failed: " << recovery_error.what() << std::endl; + spdlog::error("Recovery failed: {}", recovery_error.what()); } } - std::cout << "[JointPositionMode] Exited.\n"; + spdlog::info("[JointPositionMode] Exited."); } } @@ -63,4 +63,4 @@ void JointPositionMode::writeCommand(const protocol::ByteView& data) { void JointPositionMode::writeZeroCommand() { franka::JointPositions current_positions = current_state_->read().q; desired_positions_.write(current_positions); -} \ No newline at end of file +} diff --git a/src/control_mode/joint_velocity_mode.cpp b/src/control_mode/joint_velocity_mode.cpp index 2a19d2c..61e9cf2 100644 --- a/src/control_mode/joint_velocity_mode.cpp +++ b/src/control_mode/joint_velocity_mode.cpp @@ -1,7 +1,7 @@ #include "joint_velocity_mode.hpp" #include #include -#include +#include #include "protocol/codec.hpp" JointVelocityMode::JointVelocityMode(): @@ -11,13 +11,13 @@ JointVelocityMode::~JointVelocityMode() = default; void JointVelocityMode::controlLoop() { - std::cout << "[JointVelocityMode] Started.\n"; + spdlog::info("[JointVelocityMode] Started."); is_running_ = true; // Initialize desired velocities to zero desired_velocities_.write(franka::JointVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); if (!robot_ || !model_) { - std::cerr << "[JointVelocityMode] Robot or model not set.\n"; + spdlog::error("[JointVelocityMode] Robot or model not set."); return; } robot_->automaticErrorRecovery(); @@ -38,16 +38,16 @@ void JointVelocityMode::controlLoop() { try { robot_->control(joint_velocity_callback); } catch (const franka::ControlException& e) { - std::cerr << "[JointVelocityMode] Exception: " << e.what() << std::endl; + spdlog::error("[JointVelocityMode] Exception: {}", e.what()); if (std::string(e.what()).find("reflex") != std::string::npos) { - std::cout << "Reflex detected, attempting automatic recovery...\n"; + spdlog::warn("[JointVelocityMode] Reflex detected, attempting automatic recovery..."); try { robot_->automaticErrorRecovery(); } catch (const franka::Exception& recovery_error) { - std::cerr << "Recovery failed: " << recovery_error.what() << std::endl; + spdlog::error("Recovery failed: {}", recovery_error.what()); } } - std::cout << "[JointVelocityMode] Exited.\n"; + spdlog::info("[JointVelocityMode] Exited."); } } @@ -63,4 +63,4 @@ void JointVelocityMode::writeCommand(const protocol::ByteView& data) { void JointVelocityMode::writeZeroCommand() { desired_velocities_.write(franka::JointVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); -} \ No newline at end of file +} diff --git a/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index 26ae122..f4a940e 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -1,9 +1,9 @@ -#include #include #include #include #include #include +#include #include "franka_arm_proxy.hpp" #include "protocol/codec.hpp" #include "protocol/msg_id.hpp" @@ -20,7 +20,7 @@ static std::atomic running_flag{true}; // let ctrl-c stop the server static void signalHandler(int signum) { - std::cout << "\n[INFO] Caught signal " << signum << ", shutting down..." << std::endl; + spdlog::info("Caught signal {}, shutting down...", signum); running_flag = false; } // Todo: may add to config file later @@ -47,7 +47,7 @@ FrankaArmProxy::FrankaArmProxy(const std::string& config_path) robot_ip_ = config.getValue("robot_ip"); //bind state pub socket state_pub_addr_ = config.getValue("state_pub_addr"); - std::cout<<"state_pub: "<< state_pub_addr_ <stop(); } #endif @@ -99,8 +99,8 @@ FrankaArmProxy::~FrankaArmProxy() { bool FrankaArmProxy::start(){ is_running = true; - std::cout << is_running <<"control"<< std::endl; - std::cout << robot_<<"robot"<< std::endl; + spdlog::info("Arm proxy running flag set to {}", is_running.load()); + spdlog::info("Robot interface initialized: {}", robot_ != nullptr); #if LOCAL_TESTING current_state_.write(default_state); #else @@ -114,7 +114,7 @@ bool FrankaArmProxy::start(){ } void FrankaArmProxy::stop() { - std::cout << "[INFO] Stopping FrankaArmProxy..." << std::endl; + spdlog::info("Stopping FrankaArmProxy..."); is_running = false; if (current_mode_) current_mode_->stop(); @@ -124,7 +124,7 @@ void FrankaArmProxy::stop() { try { state_pub_socket_.close(); } catch (const zmq::error_t& e) { - std::cerr << "[ZMQ ERROR] " << e.what() << "\n"; + spdlog::error("[ZMQ ERROR] {}", e.what()); } // wait for closing service_registry_.stop(); @@ -133,19 +133,19 @@ void FrankaArmProxy::stop() { model_.reset(); #endif current_mode_ = nullptr;// reset current control mode - std::cout << "[INFO] FrankaArmProxy stopped successfully." << std::endl; + spdlog::info("FrankaArmProxy stopped successfully."); } // Main loop for processing requests, ctrl-c to stop the server void FrankaArmProxy::spin() { std::signal(SIGINT, signalHandler); // Catch Ctrl+C to stop the server - std::cout << "[INFO] Entering main spin loop. Press Ctrl+C to exit." << std::endl; - std::cout << "running_flag " << running_flag << std::endl; + spdlog::info("Entering main spin loop. Press Ctrl+C to exit."); + spdlog::info("Current running flag: {}", running_flag.load()); while (running_flag) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } stop(); - std::cout << "[INFO] Shutdown complete.\n"; + spdlog::info("Shutdown complete."); } // publish threads @@ -164,10 +164,10 @@ void FrankaArmProxy::statePublishThread() { void FrankaArmProxy::setControlMode(const protocol::FrankaArmControlMode& mode) { if (current_mode_.get() != nullptr) { - std::cout << "[Info] Stopping previous control mode...\n"; + spdlog::info("Stopping previous control mode..."); current_mode_->stop(); // stopMotion + is_running_ = false } - std::cout << "[Info] Switching to control mode: " << protocol::toString(mode.id) << " with URL: " << mode.url << std::endl; + spdlog::info("Switching to control mode: {} (command URL: {})", protocol::toString(mode.id), mode.url); current_mode_ = ControlModeFactory::create(mode.id); #if !LOCAL_TESTING current_mode_->setRobot(robot_); diff --git a/src/main.cpp b/src/main.cpp index 74ebf1a..ce3f08b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,15 +1,40 @@ -#include +#include +#include +#include +#include +#include +#include #include "franka_arm_proxy.hpp" #include "franka_gripper_proxy.hpp" int main(int argc, char **argv) { + //setup logging + //build 2 sink for both console and file logging + auto console_sink = std::make_shared(); + + std::time_t t = std::time(nullptr); + char ts[32]; + std::strftime(ts, sizeof(ts), "%Y%m%d_%H%M%S", std::localtime(&t)); + auto file_sink = std::make_shared( + std::string("logs/output_") + ts + ".txt", true + ); + + // combine sink + std::vector sinks { console_sink, file_sink }; + auto logger = std::make_shared("multi_logger", sinks.begin(), sinks.end()); + + // set default logger + spdlog::set_default_logger(logger); + spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v"); + + //check configpath argument if (argc != 2) { - std::cerr << "Please input the config path" << std::endl; - return 0; + spdlog::error("Please provide the config path as the sole argument."); + return 1; } - + //initialize and start proxies std::string config_path = argv[1]; FrankaArmProxy robot_proxy(config_path); FrankaGripperProxy gripper_proxy(config_path); From 42b84827a55f75b8ed23b335b325109b147c6c5e Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Wed, 19 Nov 2025 23:09:08 +0100 Subject: [PATCH 2/3] reconstruct logger.hpp & using logger macro --- .../control_mode/abstract_control_mode.hpp | 22 ++-- include/control_mode/control_mode_factory.hpp | 4 +- include/franka_gripper_proxy.hpp | 29 +++-- include/utils/franka_config.hpp | 8 +- include/utils/logger.hpp | 114 ++++++++++-------- include/utils/service_registry.hpp | 20 +-- src/control_mode/cartesian_pose_mode.cpp | 15 +-- src/control_mode/cartesian_velocity_mode.cpp | 14 +-- src/control_mode/human_control_mode.cpp | 18 +-- src/control_mode/idle_control_mode.cpp | 10 +- src/control_mode/joint_position_mode.cpp | 14 +-- src/control_mode/joint_velocity_mode.cpp | 14 +-- src/franka_arm_proxy.cpp | 31 +++-- src/main.cpp | 25 ++-- 14 files changed, 170 insertions(+), 168 deletions(-) diff --git a/include/control_mode/abstract_control_mode.hpp b/include/control_mode/abstract_control_mode.hpp index 86bbafa..b18ccdb 100644 --- a/include/control_mode/abstract_control_mode.hpp +++ b/include/control_mode/abstract_control_mode.hpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include "utils/logger.hpp" #include "utils/atomic_double_buffer.hpp" #include "utils/zmq_context.hpp" @@ -25,34 +25,34 @@ class AbstractControlMode { virtual void start() { startRobot(); control_thread_ = std::thread(&AbstractControlMode::controlLoop, this); - spdlog::info("[{}] Control thread launched.", getModeName()); + LOG_INFO("[{}] Control thread launched.", getModeName()); command_thread_ = std::thread(&AbstractControlMode::commandSubscriptionLoop, this); - spdlog::info("[{}] Command subscription thread launched.", getModeName()); + LOG_INFO("[{}] Command subscription thread launched.", getModeName()); }; void startRobot() { #if !LOCAL_TESTING if (!robot_ || !model_) { - spdlog::error("[{}] Robot or model not set.", getModeName()); + LOG_ERROR("[{}] Robot or model not set.", getModeName()); return; } robot_->automaticErrorRecovery(); #endif - spdlog::info("[{}] Robot control started.", getModeName()); + LOG_INFO("[{}] Robot control started.", getModeName()); is_running_ = true; }; virtual void stop() { is_running_ = false; if (control_thread_.joinable()) { - spdlog::info("[{}] Stopping control thread...", getModeName()); + LOG_INFO("[{}] Stopping control thread...", getModeName()); control_thread_.join(); } if (command_thread_.joinable()) { - spdlog::info("[{}] Stopping command subscription thread...", getModeName()); + LOG_INFO("[{}] Stopping command subscription thread...", getModeName()); command_thread_.join(); } - spdlog::info("[{}] Stopped.", getModeName()); + LOG_INFO("[{}] Stopped.", getModeName()); }; // Get the mode ID for this control mode virtual protocol::ModeID getModeID() const = 0; // Return the mode ID as an integer @@ -96,7 +96,7 @@ class AbstractControlMode { zmq::socket_t sub_socket_(ZmqContext::instance(), ZMQ_SUB); sub_socket_.set(zmq::sockopt::rcvtimeo, 200); // 100 ms timeout if (command_sub_addr_.empty()) { - spdlog::warn("[{}] Command subscription address is empty. Exiting command subscription loop.", getModeName()); + LOG_WARN("[{}] Command subscription address is empty. Exiting command subscription loop.", getModeName()); return; } sub_socket_.connect(command_sub_addr_); @@ -114,12 +114,12 @@ class AbstractControlMode { }; writeCommand(data); } catch (const zmq::error_t& e) { - spdlog::error("[FrankaProxy] ZMQ recv error: {}", e.what()); + LOG_ERROR("[FrankaProxy] ZMQ recv error: {}", e.what()); break; } } sub_socket_.close(); - spdlog::info("[{}] Command subscription loop exited.", getModeName()); + LOG_INFO("[{}] Command subscription loop exited.", getModeName()); }; virtual void writeCommand(const protocol::ByteView& data) = 0; diff --git a/include/control_mode/control_mode_factory.hpp b/include/control_mode/control_mode_factory.hpp index 8b4503b..3a25fd4 100644 --- a/include/control_mode/control_mode_factory.hpp +++ b/include/control_mode/control_mode_factory.hpp @@ -4,14 +4,14 @@ #include #include #include -#include +#include "utils/logger.hpp" class ControlModeFactory { public: static void registerMode(const std::string& name, std::function()> creator) { - spdlog::info("[ControlModeFactory] Registering mode: {}", name); + LOG_INFO("[ControlModeFactory] Registering mode: {}", name); getRegistry()[name] = std::move(creator); } diff --git a/include/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp index 51d655d..241c7e7 100644 --- a/include/franka_gripper_proxy.hpp +++ b/include/franka_gripper_proxy.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include "utils/logger.hpp" #include #include @@ -15,7 +15,7 @@ #include "utils/franka_config.hpp" #include "protocol/codec.hpp" #include "protocol/grasp_command.hpp" - +#include enum class FrankaGripperFlag { STOP = 0, STOPPING = 1, @@ -58,7 +58,7 @@ class FrankaGripperProxy { // Core server operations void start() { is_running = true; - spdlog::info("Gripper proxy running flag set to {}", is_running.load()); + LOG_INFO("Gripper proxy running flag set to {}", is_running.load()); // state_pub_thread_ = std::thread(&FrankaGripperProxy::statePublishThread, this); service_registry_.start(); command_.write(protocol::GraspCommand{(float)current_state_.read().width, 20.0f, 20.0f}); @@ -68,7 +68,7 @@ class FrankaGripperProxy { }; void stop() { - spdlog::info("Stopping FrankaGripperProxy..."); + LOG_INFO("Stopping FrankaGripperProxy..."); is_running = false; is_on_control_mode = false; if (state_pub_thread_.joinable()) state_pub_thread_.join(); @@ -79,7 +79,7 @@ class FrankaGripperProxy { // wait for closing service_registry_.stop(); gripper_.reset(); - spdlog::info("FrankaGripperProxy stopped successfully."); + LOG_INFO("FrankaGripperProxy stopped successfully."); }; private: @@ -116,18 +116,18 @@ class FrankaGripperProxy { try { if (current_width > target_width + 0.01) { - std::cout << "[Gripper Close] Closing gripper to target width: " << target_width << std::endl; + LOG_INFO("[Gripper Close] Closing gripper to target width: {}", target_width); gripper_flag.store(FrankaGripperFlag::CLOSING); gripper_->grasp(target_width, command_.read().speed, 60.0); } else if (current_width < target_width - 0.01) { - std::cout << "[Gripper Open] Opening gripper to target width: " << target_width << std::endl; + LOG_INFO("[Gripper Open] Opening gripper to target width: {}", target_width); gripper_flag.store(FrankaGripperFlag::OPENING); gripper_->move(target_width, command_.read().speed); } } catch(const std::exception& e) { - spdlog::error("{}", e.what()); + LOG_ERROR("{}", e.what()); } gripper_flag.store(FrankaGripperFlag::STOP); } @@ -149,7 +149,7 @@ class FrankaGripperProxy { try { state_pub_socket_.close(); } catch (const zmq::error_t& e) { - spdlog::error("[ZMQ ERROR] {}", e.what()); + LOG_ERROR("[ZMQ ERROR] {}", e.what()); } }; @@ -169,13 +169,13 @@ class FrankaGripperProxy { } try { - std::cout << "[Gripper Stop] Gripper reached target width: " << target_width << std::endl; + LOG_INFO("[Gripper Stop] Stopping gripper at current width: {}", current_width); gripper_flag.store(FrankaGripperFlag::STOP); gripper_->stop(); } catch(const std::exception& e) { - spdlog::error("{}", e.what()); + LOG_ERROR("{}", e.what()); } #endif std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -200,9 +200,8 @@ class FrankaGripperProxy { }; command_.write(protocol::decode(data)); // std::this_thread::sleep_for(std::chrono::milliseconds(10)); - // std::cout << "[FrankaGripperProxy] Received new gripper command: width=" - // << command_.read().width << ", speed=" << command_.read().speed - // << ", force=" << command_.read().force << std::endl; + // spdlog::info("[FrankaGripperProxy] Received new gripper command: width={}, speed={}, force={}", + // command_.read().width, command_.read().speed, command_.read().force); } }; @@ -220,7 +219,7 @@ class FrankaGripperProxy { ServiceRegistry service_registry_; void startFrankaGripperControl(const std::string& command_sub_addr) { if (is_on_control_mode) { - spdlog::warn("[FrankaGripperProxy] Already in control mode, stopping previous command subscriber."); + LOG_WARN("[FrankaGripperProxy] Already in control mode, stopping previous command subscriber."); is_on_control_mode = false; if (command_sub_thread_.joinable()) command_sub_thread_.join(); } diff --git a/include/utils/franka_config.hpp b/include/utils/franka_config.hpp index d020a63..341273b 100644 --- a/include/utils/franka_config.hpp +++ b/include/utils/franka_config.hpp @@ -1,7 +1,7 @@ -#include #include #include #include +#include "utils/logger.hpp" class FrankaConfig { public: @@ -16,7 +16,7 @@ class FrankaConfig { void loadFromFile(const std::string& file_path) { std::ifstream file(file_path); if (!file.is_open()) { - spdlog::error("Error: Could not open config file: {}", file_path); + LOG_ERROR("Error: Could not open config file: {}", file_path); return; } @@ -39,7 +39,7 @@ class FrankaConfig { if (it != config_data.end()) { return it->second; } else { - spdlog::warn("Warning: Key not found in config: {}, using default: {}", key, default_value); + LOG_WARN("Warning: Key not found in config: {}, using default: {}", key, default_value); return default_value; } } @@ -48,7 +48,7 @@ class FrankaConfig { // Function to display the loaded configuration void display() const { for (const auto& pair : config_data) { - spdlog::info("{}: {}", pair.first, pair.second); + LOG_INFO("{}: {}", pair.first, pair.second); } } diff --git a/include/utils/logger.hpp b/include/utils/logger.hpp index 3bd2bf6..e364b54 100644 --- a/include/utils/logger.hpp +++ b/include/utils/logger.hpp @@ -1,14 +1,20 @@ #pragma once + #include -#include -#include #include +#include +#include +#include #include +#include #include +#include +#include namespace utils { +// Log levels for unified control enum class LogLevel { TRACE = 0, INFO, @@ -19,50 +25,63 @@ enum class LogLevel { class Logger { public: - static Logger& instance() { - static Logger instance; - return instance; - } - - void setLevel(LogLevel level) { - level_ = level; - } + // Initialize async logging system. + // enable_file_logging = true → console + file + // enable_file_logging = false → console only + static void init(bool enable_file_logging = false, + const std::string& log_dir = "logs") + { + // Create async thread pool (1 thread, 8192 queue size) + spdlog::init_thread_pool(8192, 1); + + std::vector sinks; + + // Console sink + auto console_sink = std::make_shared(); + sinks.push_back(console_sink); + + // Optional file sink + if (enable_file_logging) { + std::filesystem::create_directories(log_dir); + + std::time_t t = std::time(nullptr); + char ts[32]; + std::strftime(ts, sizeof(ts), "%Y%m%d_%H%M%S", std::localtime(&t)); + + std::string file_path = log_dir + "/output_" + ts + ".txt"; + + auto file_sink = std::make_shared( + file_path, true + ); + sinks.push_back(file_sink); + } - void setOutputFile(const std::string& path) { - std::lock_guard lock(mutex_); - file_logger_ = spdlog::basic_logger_mt("utils_file_logger", path, true); + // Async logger using the global thread pool + auto async_logger = std::make_shared( + "proxy_async_logger", + sinks.begin(), + sinks.end(), + spdlog::thread_pool(), + spdlog::async_overflow_policy::block + ); + + // Register and set as default logger + spdlog::set_default_logger(async_logger); + + // Log formatting + // if want to see thread id:→ [%Y-%m-%d %H:%M:%S.%e] [T%t] [%^%l%$] %v + spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v"); } - void enableColor(bool /*enable*/ = true) { - // color formatting handled by spdlog's default logger pattern + // Set global logging level + static void setLevel(LogLevel lvl) { + spdlog::level::level_enum spd_lvl = toSpdLevel(lvl); + spdlog::set_level(spd_lvl); } - template - void log(LogLevel level, Args&&... args) { - if (level < level_) return; - - std::ostringstream oss; - (oss << ... << args); - - const auto spd_level = toSpdLevel(level); - if (file_logger_) { - file_logger_->log(spd_level, oss.str()); - } else { - spdlog::log(spd_level, oss.str()); - } - } - - template void trace(Args&&... a) { log(LogLevel::TRACE, std::forward(a)...); } - template void info(Args&&... a) { log(LogLevel::INFO, std::forward(a)...); } - template void warn(Args&&... a) { log(LogLevel::WARN, std::forward(a)...); } - template void error(Args&&... a) { log(LogLevel::ERROR, std::forward(a)...); } - template void fatal(Args&&... a) { log(LogLevel::FATAL, std::forward(a)...); } - private: - Logger() = default; - - static spdlog::level::level_enum toSpdLevel(LogLevel level) { - switch (level) { + static spdlog::level::level_enum toSpdLevel(LogLevel lvl) { + switch (lvl) { case LogLevel::TRACE: return spdlog::level::trace; case LogLevel::INFO: return spdlog::level::info; case LogLevel::WARN: return spdlog::level::warn; @@ -71,16 +90,13 @@ class Logger { } return spdlog::level::info; } - - LogLevel level_ = LogLevel::TRACE; - std::shared_ptr file_logger_; - std::mutex mutex_; }; } // namespace utils -#define LOG_TRACE(...) utils::Logger::instance().trace(__VA_ARGS__) -#define LOG_INFO(...) utils::Logger::instance().info(__VA_ARGS__) -#define LOG_WARN(...) utils::Logger::instance().warn(__VA_ARGS__) -#define LOG_ERROR(...) utils::Logger::instance().error(__VA_ARGS__) -#define LOG_FATAL(...) utils::Logger::instance().fatal(__VA_ARGS__) +// Logging macros using async logger (supports {} format) +#define LOG_TRACE(...) spdlog::trace(__VA_ARGS__) +#define LOG_INFO(...) spdlog::info(__VA_ARGS__) +#define LOG_WARN(...) spdlog::warn(__VA_ARGS__) +#define LOG_ERROR(...) spdlog::error(__VA_ARGS__) +#define LOG_FATAL(...) spdlog::critical(__VA_ARGS__) diff --git a/include/utils/service_registry.hpp b/include/utils/service_registry.hpp index cdd47b2..8712208 100644 --- a/include/utils/service_registry.hpp +++ b/include/utils/service_registry.hpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include "utils/logger.hpp" #include "protocol/codec.hpp" #include "protocol/request_result.hpp" #include "protocol/request_result.hpp" @@ -86,7 +86,7 @@ class ServiceRegistry { void handleRequest(const std::string& service_name, const protocol::ByteView& payload, protocol::FrankaResponse& response) { auto it = handlers_.find(service_name); - spdlog::info("[ServiceRegistry] Handling message of type {}", service_name); + LOG_INFO("[ServiceRegistry] Handling message of type {}", service_name); if (it == handlers_.end()) { // const std::string err = "Unknown handler"; // protocol::FrankaResponse rr(protocol::RequestResultCode::FAIL, err); @@ -101,10 +101,10 @@ class ServiceRegistry { try { response.payload = it->second(payload); } catch (const std::exception& e) { - spdlog::error("[ServiceRegistry] Exception while handling {} service request: {}", service_name, e.what()); + LOG_ERROR("[ServiceRegistry] Exception while handling {} service request: {}", service_name, e.what()); response.code = protocol::FrankaResponseCode::FAIL; } - spdlog::info("[ServiceRegistry] Found handler for service {}", service_name); + LOG_INFO("[ServiceRegistry] Found handler for service {}", service_name); } void clearHandlers() { @@ -120,7 +120,7 @@ class ServiceRegistry { while (is_running) { zmq::message_t service_name_msg; if (!res_socket_.recv(service_name_msg, zmq::recv_flags::none)) continue; - spdlog::debug("[FrankaArmProxy] Received service request frame of size {} bytes.", service_name_msg.size()); + LOG_TRACE("[FrankaArmProxy] Received service request frame of size {} bytes.", service_name_msg.size()); std::string service_name = protocol::decode(protocol::ByteView{ static_cast(service_name_msg.data()), service_name_msg.size() @@ -128,27 +128,27 @@ class ServiceRegistry { if (!service_name_msg.more()) { - spdlog::warn("[FrankaArmProxy] Warning: No payload frame received for service request."); + LOG_WARN("[FrankaArmProxy] Warning: No payload frame received for service request."); continue; // Skip this iteration if no payload } zmq::message_t payload_msg; if (!res_socket_.recv(payload_msg, zmq::recv_flags::none)) continue; - spdlog::debug("[FrankaArmProxy] Received payload frame of size {} bytes.", payload_msg.size()); + LOG_TRACE("[FrankaArmProxy] Received payload frame of size {} bytes.", payload_msg.size()); protocol::ByteView payload{ static_cast(payload_msg.data()), payload_msg.size() }; if (payload_msg.more()) { - spdlog::warn("[FrankaArmProxy] Warning: More message frames received than expected."); + LOG_WARN("[FrankaArmProxy] Warning: More message frames received than expected."); } - spdlog::info("[FrankaArmProxy] Received request {}", service_name); + LOG_INFO("[FrankaArmProxy] Received request {}", service_name); //std::string response; protocol::FrankaResponse response; handleRequest(service_name, payload, response); //send response res_socket_.send(zmq::buffer(service_name), zmq::send_flags::sndmore); res_socket_.send(zmq::buffer(response.payload), zmq::send_flags::none); - spdlog::info("[FrankaArmProxy] Sent response payload of {} bytes.", response.payload.size()); + LOG_INFO("[FrankaArmProxy] Sent response payload of {} bytes.", response.payload.size()); } } diff --git a/src/control_mode/cartesian_pose_mode.cpp b/src/control_mode/cartesian_pose_mode.cpp index 29e9918..1e13833 100644 --- a/src/control_mode/cartesian_pose_mode.cpp +++ b/src/control_mode/cartesian_pose_mode.cpp @@ -2,7 +2,8 @@ #include "protocol/mode_id.hpp" #include "protocol/codec.hpp" #include -#include +#include "utils/logger.hpp" +#include //Note: Pose is represented as a 4x4 matrix in column-major format. CartesianPoseMode::CartesianPoseMode(): desired_pose_(franka::CartesianPose{ @@ -15,7 +16,7 @@ CartesianPoseMode::CartesianPoseMode(): CartesianPoseMode::~CartesianPoseMode() = default; void CartesianPoseMode::controlLoop() { - spdlog::info("[CartesianPoseMode] Started."); + LOG_INFO("[CartesianPoseMode] Started."); is_running_ = true; // Initialize desired Cartesian pose to identity (no movement)` @@ -27,7 +28,7 @@ void CartesianPoseMode::controlLoop() { }); if (!robot_ || !model_) { - spdlog::error("[CartesianPoseMode] Robot or model not set."); + LOG_ERROR("[CartesianPoseMode] Robot or model not set."); return; } @@ -51,22 +52,22 @@ void CartesianPoseMode::controlLoop() { try { robot_->control(motion_generator_callback); } catch (const std::exception &ex) { - spdlog::error("[CartesianPoseMode] Robot is unable to be controlled: {}", ex.what()); + LOG_ERROR("[CartesianPoseMode] Robot is unable to be controlled: {}", ex.what()); is_robot_operational = false; } for (int i = 0; i < 3; i++) { - spdlog::warn("[CartesianPoseMode] Waiting {} seconds before recovery attempt...", 3); + LOG_WARN("[CartesianPoseMode] Waiting {} seconds before recovery attempt...", 3); // Wait usleep(1000 * 3); // Attempt recovery try { robot_->automaticErrorRecovery(); - spdlog::info("[CartesianPoseMode] Robot operation recovered."); + LOG_INFO("[CartesianPoseMode] Robot operation recovered."); is_robot_operational = true; break; } catch (const std::exception &ex) { - spdlog::error("[CartesianPoseMode] Recovery failed: {}", ex.what()); + LOG_ERROR("[CartesianPoseMode] Recovery failed: {}", ex.what()); } } diff --git a/src/control_mode/cartesian_velocity_mode.cpp b/src/control_mode/cartesian_velocity_mode.cpp index c69eae5..2e84013 100644 --- a/src/control_mode/cartesian_velocity_mode.cpp +++ b/src/control_mode/cartesian_velocity_mode.cpp @@ -3,7 +3,7 @@ #include "protocol/codec.hpp" #include #include -#include +#include "utils/logger.hpp" #include CartesianVelocityMode::CartesianVelocityMode(): @@ -12,13 +12,13 @@ CartesianVelocityMode::CartesianVelocityMode(): CartesianVelocityMode::~CartesianVelocityMode() = default; void CartesianVelocityMode::controlLoop() { - spdlog::info("[CartesianVelocityMode] Started."); + LOG_INFO("[CartesianVelocityMode] Started."); is_running_ = true; desired_velocities_.write(franka::CartesianVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); if (!robot_ || !model_) { - spdlog::error("[CartesianVelocityMode] Robot or model not set."); + LOG_ERROR("[CartesianVelocityMode] Robot or model not set."); return; } @@ -62,11 +62,11 @@ void CartesianVelocityMode::controlLoop() { // robot_->control(impedance_control_callback, motion_generator_callback, true, 1); robot_->control(motion_generator_callback, franka::ControllerMode::kCartesianImpedance, true, 1); } catch (const std::exception &ex) { - spdlog::error("[CartesianVelocityMode] Robot is unable to be controlled: {}", ex.what()); + LOG_ERROR("[CartesianVelocityMode] Robot is unable to be controlled: {}", ex.what()); is_robot_operational = false; } for (int i = 0; i < 3; i++) { - spdlog::warn("[CartesianVelocityMode] Waiting {} seconds before recovery attempt...", 3); + LOG_WARN("[CartesianVelocityMode] Waiting {} seconds before recovery attempt...", 3); // Wait usleep(1000 * 3); @@ -74,11 +74,11 @@ void CartesianVelocityMode::controlLoop() { // Attempt recovery try { robot_->automaticErrorRecovery(); - spdlog::info("[CartesianVelocityMode] Robot operation recovered."); + LOG_INFO("[CartesianVelocityMode] Robot operation recovered."); is_robot_operational = true; break; } catch (const std::exception &ex) { - spdlog::error("[CartesianVelocityMode] Recovery failed: {}", ex.what()); + LOG_ERROR("[CartesianVelocityMode] Recovery failed: {}", ex.what()); } } } diff --git a/src/control_mode/human_control_mode.cpp b/src/control_mode/human_control_mode.cpp index a911bf8..f5541b6 100644 --- a/src/control_mode/human_control_mode.cpp +++ b/src/control_mode/human_control_mode.cpp @@ -1,16 +1,16 @@ #include "human_control_mode.hpp" #include -#include +#include "utils/logger.hpp" HumanControlMode::HumanControlMode() = default; HumanControlMode::~HumanControlMode() = default; void HumanControlMode::start() { is_running_ = true; - spdlog::info("[HumanControlMode] Started."); + LOG_INFO("[HumanControlMode] Started."); if (!robot_ || !model_) { - spdlog::error("[HumanControlMode] Robot or model not set."); + LOG_ERROR("[HumanControlMode] Robot or model not set."); return; } robot_->setCollisionBehavior( @@ -36,16 +36,16 @@ void HumanControlMode::start() { try { robot_->control(callback); } catch (const franka::ControlException& e) { - spdlog::error("[HumanControlMode] Exception: {}", e.what()); + LOG_ERROR("[HumanControlMode] Exception: {}", e.what()); if (std::string(e.what()).find("reflex") != std::string::npos) { - spdlog::warn("Reflex detected, attempting automatic recovery..."); + LOG_WARN("Reflex detected, attempting automatic recovery..."); try { robot_->automaticErrorRecovery(); } catch (const franka::Exception& recovery_error) { - spdlog::error("Recovery failed: {}", recovery_error.what()); + LOG_ERROR("Recovery failed: {}", recovery_error.what()); } } - spdlog::info("[HumanControlMode] Exited."); + LOG_INFO("[HumanControlMode] Exited."); } } @@ -56,12 +56,12 @@ protocol::ModeID HumanControlMode::getModeID() const { void HumanControlMode::writeCommand(const protocol::ByteView& data) { // HumanControlMode does not process external commands; ignore incoming data. - spdlog::warn("[HumanControlMode] Received command data, but this mode does not accept commands."); + LOG_WARN("[HumanControlMode] Received command data, but this mode does not accept commands."); } void HumanControlMode::controlLoop() { // HumanControlMode control logic is handled in start(); this function is unused. - spdlog::warn("[HumanControlMode] controlLoop() called, but control is managed in start()."); + LOG_WARN("[HumanControlMode] controlLoop() called, but control is managed in start()."); } void HumanControlMode::writeZeroCommand() { diff --git a/src/control_mode/idle_control_mode.cpp b/src/control_mode/idle_control_mode.cpp index feb5ccf..609ad0f 100644 --- a/src/control_mode/idle_control_mode.cpp +++ b/src/control_mode/idle_control_mode.cpp @@ -1,5 +1,5 @@ #include "idle_control_mode.hpp" -#include +#include "utils/logger.hpp" IdleControlMode::IdleControlMode() = default; @@ -14,7 +14,7 @@ protocol::ModeID IdleControlMode::getModeID() const { void IdleControlMode::start() { startRobot(); control_thread_ = std::thread(&IdleControlMode::controlLoop, this); - spdlog::info("[{}] Control thread launched.", getModeName()); + LOG_INFO("[{}] Control thread launched.", getModeName()); } @@ -28,7 +28,7 @@ void IdleControlMode::controlLoop() { } #else if (!robot_ || !model_) { - spdlog::error("[IdleControlMode] Robot or model not set."); + LOG_ERROR("[IdleControlMode] Robot or model not set."); return; } while (is_running_) { @@ -38,10 +38,10 @@ void IdleControlMode::controlLoop() { current_state_->write(state); } } catch (const franka::Exception& e) { - spdlog::error("[IdleMode] readOnce() failed: {}", e.what()); + LOG_ERROR("[IdleMode] readOnce() failed: {}", e.what()); } } - spdlog::info("[IdleControlMode] Exited."); + LOG_INFO("[IdleControlMode] Exited."); #endif } diff --git a/src/control_mode/joint_position_mode.cpp b/src/control_mode/joint_position_mode.cpp index a6ef363..7cd0792 100644 --- a/src/control_mode/joint_position_mode.cpp +++ b/src/control_mode/joint_position_mode.cpp @@ -1,6 +1,6 @@ #include "control_mode/joint_position_mode.hpp" #include -#include +#include "utils/logger.hpp" #include "protocol/codec.hpp" JointPositionMode::JointPositionMode(): @@ -9,13 +9,13 @@ JointPositionMode::JointPositionMode(): JointPositionMode::~JointPositionMode() = default; void JointPositionMode::controlLoop() { - spdlog::info("[JointPositionMode] Started."); + LOG_INFO("[JointPositionMode] Started."); is_running_ = true; desired_positions_.write(franka::JointPositions{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); if (!robot_ || !model_) { - spdlog::error("[JointPositionMode] Robot or model not set."); + LOG_ERROR("[JointPositionMode] Robot or model not set."); return; } @@ -37,16 +37,16 @@ void JointPositionMode::controlLoop() { try { robot_->control(callback); } catch (const franka::ControlException& e) { - spdlog::error("[JointPositionMode] Exception: {}", e.what()); + LOG_ERROR("[JointPositionMode] Exception: {}", e.what()); if (std::string(e.what()).find("reflex") != std::string::npos) { - spdlog::warn("Reflex detected, attempting automatic recovery..."); + LOG_WARN("Reflex detected, attempting automatic recovery..."); try { robot_->automaticErrorRecovery(); } catch (const franka::Exception& recovery_error) { - spdlog::error("Recovery failed: {}", recovery_error.what()); + LOG_ERROR("Recovery failed: {}", recovery_error.what()); } } - spdlog::info("[JointPositionMode] Exited."); + LOG_INFO("[JointPositionMode] Exited."); } } diff --git a/src/control_mode/joint_velocity_mode.cpp b/src/control_mode/joint_velocity_mode.cpp index 61e9cf2..567274b 100644 --- a/src/control_mode/joint_velocity_mode.cpp +++ b/src/control_mode/joint_velocity_mode.cpp @@ -1,7 +1,7 @@ #include "joint_velocity_mode.hpp" #include #include -#include +#include "utils/logger.hpp" #include "protocol/codec.hpp" JointVelocityMode::JointVelocityMode(): @@ -11,13 +11,13 @@ JointVelocityMode::~JointVelocityMode() = default; void JointVelocityMode::controlLoop() { - spdlog::info("[JointVelocityMode] Started."); + LOG_INFO("[JointVelocityMode] Started."); is_running_ = true; // Initialize desired velocities to zero desired_velocities_.write(franka::JointVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); if (!robot_ || !model_) { - spdlog::error("[JointVelocityMode] Robot or model not set."); + LOG_ERROR("[JointVelocityMode] Robot or model not set."); return; } robot_->automaticErrorRecovery(); @@ -38,16 +38,16 @@ void JointVelocityMode::controlLoop() { try { robot_->control(joint_velocity_callback); } catch (const franka::ControlException& e) { - spdlog::error("[JointVelocityMode] Exception: {}", e.what()); + LOG_ERROR("[JointVelocityMode] Exception: {}", e.what()); if (std::string(e.what()).find("reflex") != std::string::npos) { - spdlog::warn("[JointVelocityMode] Reflex detected, attempting automatic recovery..."); + LOG_WARN("[JointVelocityMode] Reflex detected, attempting automatic recovery..."); try { robot_->automaticErrorRecovery(); } catch (const franka::Exception& recovery_error) { - spdlog::error("Recovery failed: {}", recovery_error.what()); + LOG_ERROR("Recovery failed: {}", recovery_error.what()); } } - spdlog::info("[JointVelocityMode] Exited."); + LOG_INFO("[JointVelocityMode] Exited."); } } diff --git a/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index 5a9fea3..864c381 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include "utils/logger.hpp" #include "franka_arm_proxy.hpp" #include "protocol/codec.hpp" #include "utils/franka_config.hpp" @@ -18,7 +18,7 @@ static std::atomic running_flag{true}; // let ctrl-c stop the server static void signalHandler(int signum) { - spdlog::info("Caught signal {}, shutting down...", signum); + LOG_INFO("Caught signal {}, shutting down...", signum); running_flag = false; } // Todo: may add to config file later @@ -45,7 +45,7 @@ FrankaArmProxy::FrankaArmProxy(const std::string& config_path) robot_ip_ = config.getValue("robot_ip"); //bind state pub socket state_pub_addr_ = config.getValue("state_pub_addr"); - spdlog::info("State publisher bound to {}", state_pub_addr_); + LOG_INFO("State publisher bound to {}", state_pub_addr_); state_pub_socket_.bind(state_pub_addr_); service_registry_.bindSocket(config.getValue("service_addr")); //initialize franka robot @@ -57,7 +57,7 @@ FrankaArmProxy::FrankaArmProxy(const std::string& config_path) } catch(const franka::NetworkException& e) { - spdlog::error("{}", e.what()); + LOG_ERROR("{}", e.what()); this->stop(); } #endif @@ -97,22 +97,19 @@ FrankaArmProxy::~FrankaArmProxy() { bool FrankaArmProxy::start(){ is_running = true; - spdlog::info("Arm proxy running flag set to {}", is_running.load()); - spdlog::info("Robot interface initialized: {}", robot_ != nullptr); + LOG_INFO("Arm proxy running flag set to {}", is_running.load()); + LOG_INFO("Robot interface initialized: {}", robot_ != nullptr); #if LOCAL_TESTING current_state_.write(default_state); #else // current_state_.write(robot_->readOnce()); #endif state_pub_thread_ = std::thread(&FrankaArmProxy::statePublishThread, this); - // std::cout << "done arm state pub"<< std::endl; - // service_thread_ = std::thread(&FrankaArmProxy::responseSocketThread, this); - // std::cout << "done service"<< std::endl; return true; } void FrankaArmProxy::stop() { - spdlog::info("Stopping FrankaArmProxy..."); + LOG_INFO("Stopping FrankaArmProxy..."); is_running = false; if (current_mode_) current_mode_->stop(); @@ -122,7 +119,7 @@ void FrankaArmProxy::stop() { try { state_pub_socket_.close(); } catch (const zmq::error_t& e) { - spdlog::error("[ZMQ ERROR] {}", e.what()); + LOG_ERROR("[ZMQ ERROR] {}", e.what()); } // wait for closing service_registry_.stop(); @@ -131,19 +128,19 @@ void FrankaArmProxy::stop() { model_.reset(); #endif current_mode_ = nullptr;// reset current control mode - spdlog::info("FrankaArmProxy stopped successfully."); + LOG_INFO("FrankaArmProxy stopped successfully."); } // Main loop for processing requests, ctrl-c to stop the server void FrankaArmProxy::spin() { std::signal(SIGINT, signalHandler); // Catch Ctrl+C to stop the server - spdlog::info("Entering main spin loop. Press Ctrl+C to exit."); - spdlog::info("Current running flag: {}", running_flag.load()); + LOG_INFO("Entering main spin loop. Press Ctrl+C to exit."); + LOG_INFO("Current running flag: {}", running_flag.load()); while (running_flag) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } stop(); - spdlog::info("Shutdown complete."); + LOG_INFO("Shutdown complete."); } // publish threads @@ -162,10 +159,10 @@ void FrankaArmProxy::statePublishThread() { void FrankaArmProxy::setControlMode(const protocol::FrankaArmControlMode& mode) { if (current_mode_.get() != nullptr) { - spdlog::info("Stopping previous control mode..."); + LOG_INFO("Stopping previous control mode..."); current_mode_->stop(); // stopMotion + is_running_ = false } - spdlog::info("Switching to control mode: {} (command URL: {})", protocol::toString(mode.id), mode.url); + LOG_INFO("Switching to control mode: {} (command URL: {})", protocol::toString(mode.id), mode.url); current_mode_ = ControlModeFactory::create(mode.id); #if !LOCAL_TESTING current_mode_->setRobot(robot_); diff --git a/src/main.cpp b/src/main.cpp index ce3f08b..d4a2a96 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,34 +4,23 @@ #include #include #include +#include "utils/logger.hpp" #include "franka_arm_proxy.hpp" #include "franka_gripper_proxy.hpp" int main(int argc, char **argv) { - //setup logging - //build 2 sink for both console and file logging - auto console_sink = std::make_shared(); - - std::time_t t = std::time(nullptr); - char ts[32]; - std::strftime(ts, sizeof(ts), "%Y%m%d_%H%M%S", std::localtime(&t)); - auto file_sink = std::make_shared( - std::string("logs/output_") + ts + ".txt", true - ); + //initialize logger + utils::Logger::init(false);//true to enable file logging + utils::Logger::setLevel(utils::LogLevel::INFO); + + - // combine sink - std::vector sinks { console_sink, file_sink }; - auto logger = std::make_shared("multi_logger", sinks.begin(), sinks.end()); - // set default logger - spdlog::set_default_logger(logger); - spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v"); - //check configpath argument if (argc != 2) { - spdlog::error("Please provide the config path as the sole argument."); + LOG_ERROR("Please provide the config path as the sole argument."); return 1; } //initialize and start proxies From a4f02f7fe0a969b7199139fd4bf06297e7060d3f Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Thu, 20 Nov 2025 16:44:15 +0100 Subject: [PATCH 3/3] reconstruction the config & add setCollisionBehavior --- P10_franka.yaml | 41 +++++++ include/franka_arm_proxy.hpp | 6 +- include/franka_gripper_proxy.hpp | 46 ++++--- include/utils/franka_config.hpp | 203 ++++++++++++++++++++++++------- src/franka_arm_proxy.cpp | 69 ++++++++--- src/main.cpp | 7 +- 6 files changed, 291 insertions(+), 81 deletions(-) create mode 100644 P10_franka.yaml diff --git a/P10_franka.yaml b/P10_franka.yaml new file mode 100644 index 0000000..5bb4e36 --- /dev/null +++ b/P10_franka.yaml @@ -0,0 +1,41 @@ +######################## +# communication +######################## +robot_ip: 172.16.1.2 +service_addr: tcp://127.0.0.1:5555 +state_pub_addr: tcp://127.0.0.1:5556 +gripper_ip: 172.16.1.3 +gripper_service_addr: tcp://127.0.0.1:5557 +gripper_state_pub_addr: tcp://127.0.0.1:5558 + + +######################## +# arm +######################## +# initial joint/matrix state +arm_default_state_q: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785] +arm_default_state_O_T_EE: [1.0, 0.0, 0.0, 0.3, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.5, 0.0, 0.0, 0.0, 1.0] +# rate/timeouts +arm_state_pub_rate_hz: 100 +arm_socket_timeout_ms: 100 +arm_max_message_size: 4096 +# collision behavior thresholds +arm_collision_lower_torque_thresholds_acc: [30, 30, 30, 30, 30, 30, 30] +arm_collision_upper_torque_thresholds_acc: [45, 45, 45, 45, 45, 45, 45] +arm_collision_lower_torque_thresholds_nom: [25, 25, 25, 25, 25, 25, 25] +arm_collision_upper_torque_thresholds_nom: [35, 35, 35, 35, 35, 35, 35] +arm_collision_lower_force_thresholds_acc: [10, 10, 10, 10, 10, 10] +arm_collision_upper_force_thresholds_acc: [15, 15, 15, 15, 15, 15] +arm_collision_lower_force_thresholds_nom: [8, 8, 8, 8, 8, 8] +arm_collision_upper_force_thresholds_nom: [12, 12, 12, 12, 12, 12] + + +######################## +# gripper +######################## +gripper_pub_rate_hz: 100 +gripper_command_rcvtimeo_ms: 500 +gripper_default_close_open_threshold: 0.01 +gripper_default_speed_slow: 0.01 +gripper_default_speed_fast: 20.0 +gripper_default_force: 20.0 diff --git a/include/franka_arm_proxy.hpp b/include/franka_arm_proxy.hpp index 939863a..965ab55 100644 --- a/include/franka_arm_proxy.hpp +++ b/include/franka_arm_proxy.hpp @@ -13,13 +13,14 @@ #include "control_mode/abstract_control_mode.hpp" #include "utils/atomic_double_buffer.hpp" #include "utils/service_registry.hpp" +#include "utils/franka_config.hpp" #include "protocol/codec.hpp" class FrankaArmProxy { public: // Constructor & Destructor - explicit FrankaArmProxy(const std::string& config_path);// Constructor that initializes the proxy with a configuration file + explicit FrankaArmProxy(const FrankaConfigData& config);// Constructor that initializes the proxy with a configuration file ~FrankaArmProxy();// Destructor to clean up resources // Core server operations @@ -76,6 +77,9 @@ class FrankaArmProxy { // Current robot state AtomicDoubleBuffer current_state_; + FrankaConfigData config_; + franka::RobotState default_state_; + // initialize void initializeControlMode(); void initializeService(); diff --git a/include/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp index 241c7e7..232b745 100644 --- a/include/franka_gripper_proxy.hpp +++ b/include/franka_gripper_proxy.hpp @@ -28,26 +28,34 @@ class FrankaGripperProxy { public: // Constructor & Destructor - explicit FrankaGripperProxy(const std::string& config_path): + explicit FrankaGripperProxy(const FrankaConfigData& config): is_running(false), is_on_control_mode(false), gripper_flag(FrankaGripperFlag::STOP), current_state_(AtomicDoubleBuffer(franka::GripperState{})), - command_(AtomicDoubleBuffer(protocol::GraspCommand{})) + command_(AtomicDoubleBuffer(protocol::GraspCommand{})), + config_(config) { - FrankaConfig config(config_path); - gripper_ip_ = config.getValue("gripper_ip"); - state_pub_addr_ = config.getValue("gripper_state_pub_addr"); - service_registry_.bindSocket(config.getValue("gripper_service_addr")); + gripper_ip_ = config_.gripper_ip; + state_pub_addr_ = config_.gripper_state_pub_addr; + service_registry_.bindSocket(config_.gripper_service_addr); //initialize franka gripper # if !LOCAL_TESTING gripper_ = std::make_shared(gripper_ip_); gripper_->homing(); current_state_.write(gripper_->readOnce()); - command_.write(protocol::GraspCommand{current_state_.read().width, 0.01f, 20.0f}); + command_.write(protocol::GraspCommand{ + current_state_.read().width, + static_cast(config_.gripper_default_speed_slow), + static_cast(config_.gripper_default_force) + }); # else current_state_.write(franka::GripperState{0.0f, 0.0f, false, false, franka::Duration{}}); - command_.write(protocol::GraspCommand{0.0f, 0.01f, 20.0f}); + command_.write(protocol::GraspCommand{ + 0.0f, + static_cast(config_.gripper_default_speed_slow), + static_cast(config_.gripper_default_force) + }); # endif initializeService(); }; @@ -61,7 +69,11 @@ class FrankaGripperProxy { LOG_INFO("Gripper proxy running flag set to {}", is_running.load()); // state_pub_thread_ = std::thread(&FrankaGripperProxy::statePublishThread, this); service_registry_.start(); - command_.write(protocol::GraspCommand{(float)current_state_.read().width, 20.0f, 20.0f}); + command_.write(protocol::GraspCommand{ + (float)current_state_.read().width, + static_cast(config_.gripper_default_speed_fast), + static_cast(config_.gripper_default_force) + }); state_pub_thread_ = std::thread(&FrankaGripperProxy::statePubThread, this); check_thread_ = std::thread(&FrankaGripperProxy::checkWidthThread, this); control_thread_ = std::thread(&FrankaGripperProxy::controlLoopThread, this); @@ -115,11 +127,11 @@ class FrankaGripperProxy { } try { - if (current_width > target_width + 0.01) { + if (current_width > target_width + config_.gripper_default_close_open_threshold) { LOG_INFO("[Gripper Close] Closing gripper to target width: {}", target_width); gripper_flag.store(FrankaGripperFlag::CLOSING); gripper_->grasp(target_width, command_.read().speed, 60.0); - } else if (current_width < target_width - 0.01) { + } else if (current_width < target_width - config_.gripper_default_close_open_threshold) { LOG_INFO("[Gripper Open] Opening gripper to target width: {}", target_width); gripper_flag.store(FrankaGripperFlag::OPENING); gripper_->move(target_width, command_.read().speed); @@ -144,7 +156,8 @@ class FrankaGripperProxy { protocol::encode(gs).size()), zmq::send_flags::none); #endif - std::this_thread::sleep_for(std::chrono::milliseconds(1000 / GRIPPER_PUB_RATE_HZ)); + const int rate = config_.gripper_pub_rate_hz > 0 ? config_.gripper_pub_rate_hz : GRIPPER_PUB_RATE_HZ; + std::this_thread::sleep_for(std::chrono::milliseconds(1000 / rate)); } try { state_pub_socket_.close(); @@ -160,9 +173,9 @@ class FrankaGripperProxy { float target_width = command_.read().width; if ( gripper_flag.load() == FrankaGripperFlag::STOPPING - || (gripper_flag.load() == FrankaGripperFlag::CLOSING && current_width >= target_width + 0.01) - || (gripper_flag.load() == FrankaGripperFlag::OPENING && current_width <= target_width - 0.01) - || (gripper_flag.load() == FrankaGripperFlag::STOP && std::abs(current_width - target_width) <= 0.01) + || (gripper_flag.load() == FrankaGripperFlag::CLOSING && current_width >= target_width + config_.gripper_default_close_open_threshold) + || (gripper_flag.load() == FrankaGripperFlag::OPENING && current_width <= target_width - config_.gripper_default_close_open_threshold) + || (gripper_flag.load() == FrankaGripperFlag::STOP && std::abs(current_width - target_width) <= config_.gripper_default_close_open_threshold) ) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); continue; @@ -185,7 +198,7 @@ class FrankaGripperProxy { void commandSubThread(const std::string& command_sub_addr) { is_on_control_mode = true; zmq::socket_t sub_socket_(ZmqContext::instance(), ZMQ_SUB); - sub_socket_.set(zmq::sockopt::rcvtimeo, 500); // 0.5 second timeout + sub_socket_.set(zmq::sockopt::rcvtimeo, config_.gripper_command_rcvtimeo_ms); // timeout sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages sub_socket_.connect(command_sub_addr); while (is_running && is_on_control_mode) { @@ -214,6 +227,7 @@ class FrankaGripperProxy { AtomicDoubleBuffer current_state_; AtomicDoubleBuffer command_; + FrankaConfigData config_; // service registry ServiceRegistry service_registry_; diff --git a/include/utils/franka_config.hpp b/include/utils/franka_config.hpp index 341273b..437f9bd 100644 --- a/include/utils/franka_config.hpp +++ b/include/utils/franka_config.hpp @@ -1,65 +1,184 @@ -#include +#pragma once +#include +#include #include -#include +#include +#include #include "utils/logger.hpp" +struct FrankaConfigData { + // communication + std::string robot_ip; + std::string state_pub_addr; + std::string service_addr; + std::string gripper_ip; + std::string gripper_state_pub_addr; + std::string gripper_service_addr; + + // arm + std::vector arm_default_state_q; + std::array arm_default_state_O_T_EE{}; + int arm_state_pub_rate_hz{100}; + int arm_socket_timeout_ms{100}; + int arm_max_message_size{4096}; + // collision behavior thresholds + std::array arm_col_lower_torque_acc{}; + std::array arm_col_upper_torque_acc{}; + std::array arm_col_lower_torque_nom{}; + std::array arm_col_upper_torque_nom{}; + std::array arm_col_lower_force_acc{}; + std::array arm_col_upper_force_acc{}; + std::array arm_col_lower_force_nom{}; + std::array arm_col_upper_force_nom{}; + + // gripper + int gripper_pub_rate_hz{100}; + int gripper_command_rcvtimeo_ms{500}; + double gripper_default_close_open_threshold{0.01}; + double gripper_default_speed_slow{0.01}; + double gripper_default_speed_fast{20.0}; + double gripper_default_force{20.0}; +}; + class FrankaConfig { public: - std::map config_data; - - // Constructor to initialize and load the configuration from a file - FrankaConfig(const std::string& file_path) { + FrankaConfig() = default; + explicit FrankaConfig(const std::string& file_path) { loadFromFile(file_path); } - // Function to load configuration from a file void loadFromFile(const std::string& file_path) { - std::ifstream file(file_path); - if (!file.is_open()) { - LOG_ERROR("Error: Could not open config file: {}", file_path); - return; + try { + YAML::Node node = YAML::LoadFile(file_path); + parse(node); + } catch (const YAML::BadFile& e) { + LOG_ERROR("Error: Could not open config file: {} ({})", file_path, e.what()); + } catch (const std::exception& e) { + LOG_ERROR("Error: Failed to parse config: {}", e.what()); } + } - std::string line; - while (std::getline(file, line)) { - size_t delimiter_pos = line.find(':'); - if (delimiter_pos != std::string::npos) { - std::string key = trim(line.substr(0, delimiter_pos)); - std::string value = trim(line.substr(delimiter_pos + 1)); - config_data[key] = value; - } + const FrankaConfigData& data() const { return data_; } + +private: + template + static T getOr(const YAML::Node& node, const std::string& key, const T& fallback) { + if (!node[key]) { + return fallback; + } + try { + return node[key].as(); + } catch (const std::exception& e) { + LOG_WARN("Config parse warning for key {}: {}", key, e.what()); + return fallback; } - file.close(); } - // Function to get a value as a string - - std::string getValue(const std::string& key, const std::string& default_value = "") const { - auto it = config_data.find(key); - if (it != config_data.end()) { - return it->second; - } else { - LOG_WARN("Warning: Key not found in config: {}, using default: {}", key, default_value); - return default_value; + static std::vector getVecOrEmpty(const YAML::Node& node, const std::string& key) { + if (!node[key] || !node[key].IsSequence()) { + return {}; + } + try { + return node[key].as>(); + } catch (const std::exception& e) { + LOG_WARN("Config parse warning for key {}: {}", key, e.what()); + return {}; } - } + } + static std::array getArray16OrDefault(const YAML::Node& node, const std::string& key, const std::array& fallback) { + if (!node[key] || !node[key].IsSequence()) { + return fallback; + } + try { + auto vec = node[key].as>(); + if (vec.size() != 16) { + LOG_WARN("Config key {} expected 16 values, got {}. Using fallback.", key, vec.size()); + return fallback; + } + std::array arr{}; + std::copy(vec.begin(), vec.end(), arr.begin()); + return arr; + } catch (const std::exception& e) { + LOG_WARN("Config parse warning for key {}: {}", key, e.what()); + return fallback; + } + } - // Function to display the loaded configuration - void display() const { - for (const auto& pair : config_data) { - LOG_INFO("{}: {}", pair.first, pair.second); + template + static std::array getArrayNOrDefault(const YAML::Node& node, const std::string& key, const std::array& fallback) { + if (!node[key] || !node[key].IsSequence()) { + return fallback; + } + try { + auto vec = node[key].as>(); + if (vec.size() != N) { + LOG_WARN("Config key {} expected {} values, got {}. Using fallback.", key, N, vec.size()); + return fallback; + } + std::array arr{}; + std::copy(vec.begin(), vec.end(), arr.begin()); + return arr; + } catch (const std::exception& e) { + LOG_WARN("Config parse warning for key {}: {}", key, e.what()); + return fallback; } } -private: - // Function to trim whitespace from a string - std::string trim(const std::string& str) const { - size_t first = str.find_first_not_of(" \t"); - size_t last = str.find_last_not_of(" \t"); - if (first == std::string::npos || last == std::string::npos) { - return ""; + void parse(const YAML::Node& node) { + // defaults for arrays + const std::array default_O_T_EE{{ + 1.0, 0.0, 0.0, 0.3, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.5, + 0.0, 0.0, 0.0, 1.0 + }}; + const std::vector default_q{{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}}; + + // communication + data_.robot_ip = getOr(node, "robot_ip", ""); + data_.state_pub_addr = getOr(node, "state_pub_addr", ""); + data_.service_addr = getOr(node, "service_addr", ""); + data_.gripper_ip = getOr(node, "gripper_ip", ""); + data_.gripper_state_pub_addr = getOr(node, "gripper_state_pub_addr", ""); + data_.gripper_service_addr = getOr(node, "gripper_service_addr", ""); + + // arm + data_.arm_default_state_q = getVecOrEmpty(node, "arm_default_state_q"); + if (data_.arm_default_state_q.empty()) { + data_.arm_default_state_q = default_q; } - return str.substr(first, last - first + 1); + data_.arm_default_state_O_T_EE = getArray16OrDefault(node, "arm_default_state_O_T_EE", default_O_T_EE); + data_.arm_state_pub_rate_hz = getOr(node, "arm_state_pub_rate_hz", 100); + data_.arm_socket_timeout_ms = getOr(node, "arm_socket_timeout_ms", 100); + data_.arm_max_message_size = getOr(node, "arm_max_message_size", 4096); + // collision behavior (defaults simple non-zero thresholds to avoid 0) + const std::array d7_acc_low{{30,30,30,30,30,30,30}}; + const std::array d7_acc_up {{45,45,45,45,45,45,45}}; + const std::array d7_nom_low{{25,25,25,25,25,25,25}}; + const std::array d7_nom_up {{35,35,35,35,35,35,35}}; + const std::array d6_acc_low{{10,10,10,10,10,10}}; + const std::array d6_acc_up {{15,15,15,15,15,15}}; + const std::array d6_nom_low{{8,8,8,8,8,8}}; + const std::array d6_nom_up {{12,12,12,12,12,12}}; + data_.arm_col_lower_torque_acc = getArrayNOrDefault<7>(node, "arm_collision_lower_torque_thresholds_acc", d7_acc_low); + data_.arm_col_upper_torque_acc = getArrayNOrDefault<7>(node, "arm_collision_upper_torque_thresholds_acc", d7_acc_up); + data_.arm_col_lower_torque_nom = getArrayNOrDefault<7>(node, "arm_collision_lower_torque_thresholds_nom", d7_nom_low); + data_.arm_col_upper_torque_nom = getArrayNOrDefault<7>(node, "arm_collision_upper_torque_thresholds_nom", d7_nom_up); + data_.arm_col_lower_force_acc = getArrayNOrDefault<6>(node, "arm_collision_lower_force_thresholds_acc", d6_acc_low); + data_.arm_col_upper_force_acc = getArrayNOrDefault<6>(node, "arm_collision_upper_force_thresholds_acc", d6_acc_up); + data_.arm_col_lower_force_nom = getArrayNOrDefault<6>(node, "arm_collision_lower_force_thresholds_nom", d6_nom_low); + data_.arm_col_upper_force_nom = getArrayNOrDefault<6>(node, "arm_collision_upper_force_thresholds_nom", d6_nom_up); + + // gripper + data_.gripper_pub_rate_hz = getOr(node, "gripper_pub_rate_hz", 100); + data_.gripper_command_rcvtimeo_ms = getOr(node, "gripper_command_rcvtimeo_ms", 500); + data_.gripper_default_close_open_threshold = getOr(node, "gripper_default_close_open_threshold", 0.01); + data_.gripper_default_speed_slow = getOr(node, "gripper_default_speed_slow", 0.01); + data_.gripper_default_speed_fast = getOr(node, "gripper_default_speed_fast", 20.0); + data_.gripper_default_force = getOr(node, "gripper_default_force", 20.0); } + +private: + FrankaConfigData data_; }; diff --git a/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index 864c381..cc2c2f7 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -21,39 +22,66 @@ static void signalHandler(int signum) { LOG_INFO("Caught signal {}, shutting down...", signum); running_flag = false; } -// Todo: may add to config file later -franka::RobotState default_state = []{ - franka::RobotState state; - state.q = {{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}}; - state.O_T_EE = {{ - 1.0, 0.0, 0.0, 0.3, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.5, - 0.0, 0.0, 0.0, 1.0 - }}; +namespace { +franka::RobotState makeDefaultState(const FrankaConfigData& cfg) { + franka::RobotState state{}; + std::array q{}; + const auto& q_src = cfg.arm_default_state_q.size() == 7 + ? cfg.arm_default_state_q + : std::vector{{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}}; + std::copy(q_src.begin(), q_src.begin() + 7, q.begin()); + state.q = q; + + std::array pose = cfg.arm_default_state_O_T_EE; + if (pose == std::array{}) { + pose = {{ + 1.0, 0.0, 0.0, 0.3, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.5, + 0.0, 0.0, 0.0, 1.0 + }}; + } + state.O_T_EE = pose; return state; -}(); +} +} -FrankaArmProxy::FrankaArmProxy(const std::string& config_path) +FrankaArmProxy::FrankaArmProxy(const FrankaConfigData& config) : state_pub_socket_(ZmqContext::instance(), ZMQ_PUB),//arm state publish socket is_running(false), - current_state_(AtomicDoubleBuffer(default_state)), + current_state_(AtomicDoubleBuffer(makeDefaultState(config))), + config_(config), + default_state_(makeDefaultState(config)), service_registry_() { - FrankaConfig config(config_path); - // type_ = config.getValue("type", "Arm"); // Default to "Arm" if not specified - robot_ip_ = config.getValue("robot_ip"); + robot_ip_ = config_.robot_ip; //bind state pub socket - state_pub_addr_ = config.getValue("state_pub_addr"); + state_pub_addr_ = config_.state_pub_addr; LOG_INFO("State publisher bound to {}", state_pub_addr_); state_pub_socket_.bind(state_pub_addr_); - service_registry_.bindSocket(config.getValue("service_addr")); + service_registry_.bindSocket(config_.service_addr); //initialize franka robot #if !LOCAL_TESTING try { robot_ = std::make_shared(robot_ip_); model_ = std::make_shared(robot_->loadModel()); + // set collision behavior thresholds from config + try { + robot_->setCollisionBehavior( + config_.arm_col_lower_torque_acc, + config_.arm_col_upper_torque_acc, + config_.arm_col_lower_torque_nom, + config_.arm_col_upper_torque_nom, + config_.arm_col_lower_force_acc, + config_.arm_col_upper_force_acc, + config_.arm_col_lower_force_nom, + config_.arm_col_upper_force_nom + ); + } catch (const franka::CommandException& e) { + LOG_ERROR("Failed to set collision behavior: {}", e.what()); + throw; + } } catch(const franka::NetworkException& e) { @@ -100,7 +128,7 @@ bool FrankaArmProxy::start(){ LOG_INFO("Arm proxy running flag set to {}", is_running.load()); LOG_INFO("Robot interface initialized: {}", robot_ != nullptr); #if LOCAL_TESTING - current_state_.write(default_state); + current_state_.write(default_state_); #else // current_state_.write(robot_->readOnce()); #endif @@ -152,7 +180,8 @@ void FrankaArmProxy::statePublishThread() { const std::vector payload = protocol::encode(rs); // Publish over ZMQ PUB socket state_pub_socket_.send(zmq::buffer(payload), zmq::send_flags::none); - std::this_thread::sleep_for(std::chrono::milliseconds(1000 / STATE_PUB_RATE_HZ)); + const int rate = config_.arm_state_pub_rate_hz > 0 ? config_.arm_state_pub_rate_hz : STATE_PUB_RATE_HZ; + std::this_thread::sleep_for(std::chrono::milliseconds(1000 / rate)); // std::cout << "[FrankaArmProxy] Published state message, size = " << frame.size() << " bytes." << std::endl;//debug } } diff --git a/src/main.cpp b/src/main.cpp index d4a2a96..207f369 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,6 +5,7 @@ #include #include #include "utils/logger.hpp" +#include "utils/franka_config.hpp" #include "franka_arm_proxy.hpp" #include "franka_gripper_proxy.hpp" @@ -25,8 +26,10 @@ int main(int argc, char **argv) } //initialize and start proxies std::string config_path = argv[1]; - FrankaArmProxy robot_proxy(config_path); - FrankaGripperProxy gripper_proxy(config_path); + FrankaConfig config(config_path); + const auto& cfg = config.data(); + FrankaArmProxy robot_proxy(cfg); + FrankaGripperProxy gripper_proxy(cfg); robot_proxy.start(); gripper_proxy.start(); robot_proxy.spin();