From 88738584d3845edc781a973d14d7f7f9e687cfae Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Thu, 6 Nov 2025 14:00:12 +0100 Subject: [PATCH] restruct the msgid, mode_id, message_header according to new protocol, add request_result class --- include/FrankaProxy.hpp | 26 +++++---- include/protocol/codec.hpp | 23 ++++++-- include/protocol/message_header.hpp | 29 +++++++--- include/protocol/mode_id.hpp | 48 ++++++++++++---- include/protocol/msg_id.hpp | 69 +++++++++++++++++------ include/protocol/request_result.hpp | 70 +++++++++++++++++++++++ src/FrankaProxy.cpp | 10 ++-- src/protocol/codec.cpp | 61 ++++++++++++-------- src/protocol/message_header.cpp | 47 +++++++++++++--- src/protocol/request_result.cpp | 86 +++++++++++++++++++++++++++++ 10 files changed, 382 insertions(+), 87 deletions(-) create mode 100644 include/protocol/request_result.hpp create mode 100644 src/protocol/request_result.cpp diff --git a/include/FrankaProxy.hpp b/include/FrankaProxy.hpp index 3f7fbd6..9ea2490 100644 --- a/include/FrankaProxy.hpp +++ b/include/FrankaProxy.hpp @@ -80,24 +80,28 @@ class FrankaProxy { // ZMQ communication zmq::context_t context_; + zmq::socket_t pub_arm_socket_;//arm state publish socket + zmq::socket_t sub_cmd_socket_;//command socket + zmq::socket_t rep_socket_;//service socket //zmq::socket_t pub_socket_; - zmq::socket_t pub_arm_socket_; - zmq::socket_t pub_gripper_socket_; + // zmq::socket_t pub_gripper_socket_; //zmq::socket_t sub_socket_; - zmq::socket_t sub_arm_socket_; - zmq::socket_t sub_gripper_socket_; - zmq::socket_t rep_socket_; + // zmq::socket_t sub_gripper_socket_; + // Threading - std::thread control_thread_;//controlLoopThread() - std::thread state_pub_thread_;//statePublishThread() - std::thread gripper_pub_thread_;//gripperPublishThread() - + // std::thread state_sub_thread_;//stateSubscribeThread(),just for follower + std::thread command_sub_thread_;//commandSubscribeThread(),just for follower + std::thread control_thread_;//controlLoopThread() std::thread service_thread_;//responseSocketThread() + + // std::thread gripper_pub_thread_;//gripperPublishThread() + + - std::thread state_sub_thread_;//stateSubscribeThread(),just for follower - std::thread gripper_sub_thread_;//gripperSubscribeThread(),just for follower + + // std::thread gripper_sub_thread_;//gripperSubscribeThread(),just for follower // Synchronization diff --git a/include/protocol/codec.hpp b/include/protocol/codec.hpp index c222489..9e2f31e 100644 --- a/include/protocol/codec.hpp +++ b/include/protocol/codec.hpp @@ -13,6 +13,8 @@ namespace protocol { +class RequestResult; // forward declaration? + // encode std::array (fixed size) template inline void encode_array_f64(uint8_t*& ptr, const std::array& in) { @@ -117,14 +119,27 @@ inline bool decode_bool(const uint8_t*& ptr) { std::vector encodeMessage(const MessageHeader& header, const std::vector& payload); std::vector encodeStateMessage(const FrankaArmState& state); std::vector encodeModeMessage(uint8_t mode_code); -std::vector encodeErrorMessage(uint8_t error_code); -std::vector encodeGripperMessage(const FrankaGripperState& gripper_state); +//Todo:check if need vector of following 2 functions +std::vector encodeRequestResultMessage(const RequestResult& result); // Todo: implement RequestResult class +std::vector encodePubPortMessage(uint16_t Pubport);//Todo:implement, and whta Pubport old name +//Todo:check if return bool +bool decodeModeMessage(const std::vector& data, uint8_t& mode_code);//Todo:change into FrankaArmControl +bool decodeCommandMessage(const std::vector& data, uint8_t& command); +//Todo: think of the exact command class name? check with the header 11 for Command +//Than the exact type of commmand should be switch by the following 0-5 + + + +//May no use in future bool decodeStateMessage(const std::vector& data, FrankaArmState& arm_state); +// std::vector encodeErrorMessage(uint8_t error_code); + +std::vector encodeGripperMessage(const FrankaGripperState& gripper_state); bool decodeGripperMessage(const std::vector& data, FrankaGripperState& gripper_state); -std::vector encodeStartControlResp(bool success, ModeID mode_id); +// std::vector encodeStartControlResp(bool success, ModeID mode_id); } // namespace protocol -#endif // CODEC_HPP \ No newline at end of file +#endif // CODEC_HPP diff --git a/include/protocol/message_header.hpp b/include/protocol/message_header.hpp index c3332cd..0be3d08 100644 --- a/include/protocol/message_header.hpp +++ b/include/protocol/message_header.hpp @@ -4,20 +4,35 @@ #include namespace protocol { +#include struct MessageHeader { - uint8_t id; // Message ID (1 byte) - uint16_t len; // Payload length (2 bytes) - uint8_t pad = 0;// Always 0 (1 byte) + uint8_t message_type; // Data type (see MsgID table) - uint8 + uint8_t flags; // Bit flags for message options - uint8 + uint16_t payload_length; // Payload length in bytes - uint16 + uint64_t timestamp; // High-precision timestamp (ns) - uint64 - static constexpr size_t SIZE = 4; + // Serialized header size: 1 + 1 + 2 + 8 = 12 bytes + static constexpr size_t SIZE = 12; - void encode(uint8_t* buffer) const; - - + static MessageHeader decode(const uint8_t* buffer); }; +// struct MessageHeader { +// uint8_t id; // Message ID (1 byte) +// uint16_t len; // Payload length (2 bytes) +// uint8_t pad = 0;// Always 0 (1 byte) + +// static constexpr size_t SIZE = 4; + + +// void encode(uint8_t* buffer) const; + + +// static MessageHeader decode(const uint8_t* buffer); +// }; + } // namespace protocol #endif // MESSAGE_HEADER_HPP diff --git a/include/protocol/mode_id.hpp b/include/protocol/mode_id.hpp index 524f3ab..0c9ee32 100644 --- a/include/protocol/mode_id.hpp +++ b/include/protocol/mode_id.hpp @@ -5,24 +5,50 @@ namespace protocol { +#include +#include + enum class ModeID : uint8_t { - CARTESIAN_POSITION = 0, - CARTESIAN_VELOCITY = 1, - JOINT_POSITION = 2, - JOINT_VELOCITY = 3, - HUMAN_MODE = 4, - IDLE = 5, - PD_TEST = 6, + IDLE = 0x00, // Robot idle / no control command + JOINT_POSITION = 0x01, // Position control in joint space + JOINT_VELOCITY = 0x02, // Velocity control in joint space + CARTESIAN_POSE = 0x03, // Pose control in Cartesian space + CARTESIAN_VELOCITY = 0x04, // Velocity control in Cartesian space + JOINT_TORQUE = 0x05, // Joint torque control mode + GRAVITY_COMP = 0x06 // Gravity compensation mode }; inline std::string toString(ModeID mode) { switch (mode) { - case ModeID::IDLE: return "idle"; - case ModeID::HUMAN_MODE: return "zero_torque"; - case ModeID::PD_TEST: return "joint_pd"; - default: return "idle"; + case ModeID::IDLE: return "IDLE"; + case ModeID::JOINT_POSITION: return "JOINT_POSITION"; + case ModeID::JOINT_VELOCITY: return "JOINT_VELOCITY"; + case ModeID::CARTESIAN_POSE: return "CARTESIAN_POSE"; + case ModeID::CARTESIAN_VELOCITY: return "CARTESIAN_VELOCITY"; + case ModeID::JOINT_TORQUE: return "JOINT_TORQUE"; + case ModeID::GRAVITY_COMP: return "GRAVITY_COMP"; + default: return "UNKNOWN_MODE"; } } +// enum class ModeID : uint8_t { +// CARTESIAN_POSITION = 0, +// CARTESIAN_VELOCITY = 1, +// JOINT_POSITION = 2, +// JOINT_VELOCITY = 3, +// HUMAN_MODE = 4, +// IDLE = 5, +// PD_TEST = 6, +// }; + +// inline std::string toString(ModeID mode) { +// switch (mode) { +// case ModeID::IDLE: return "idle"; +// case ModeID::HUMAN_MODE: return "zero_torque"; +// case ModeID::PD_TEST: return "joint_pd"; +// default: return "idle"; +// } +// } + } // namespace protocol #endif // MODE_ID_HPP \ No newline at end of file diff --git a/include/protocol/msg_id.hpp b/include/protocol/msg_id.hpp index d0c5632..3763689 100644 --- a/include/protocol/msg_id.hpp +++ b/include/protocol/msg_id.hpp @@ -3,25 +3,60 @@ #include namespace protocol { + enum class MsgID : uint8_t { + // ===== Client → Server : Service Request ===== + GET_FRANKA_ARM_STATE = 0, // Request a single state + GET_FRANKA_ARM_CONTROL_MODE = 1, // Ask for active control mode + SET_FRANKA_ARM_CONTROL_MODE = 2, // Switch to desired mode + GET_FRANKA_ARM_STATE_PUB_PORT = 3, // Query PUB port number + MOVE_FRANKA_ARM_TO_JOINT_POSITION = 4, // Move robot to a specific joint position + MOVE_FRANKA_ARM_TO_CARTESIAN_POSITION = 5// Move robot to a specific cartesian position + GET_FRANKA_GRIPPER_STATE = 6, // Request a single state of gripper + MOVE_FRANKA_GRIPPER = 7, // Move gripper -enum class MsgID : uint8_t { - // Client → Server - GET_STATE_REQ = 0x01, // Request a single FrankaArmState - GET_CONTROL_MODE_REQ = 0x02, // Ask for active control mode - SET_CONTROL_MODE_REQ = 0x03, // Switch to desired mode - GET_SUB_PORT_REQ = 0x04, // Query PUB port number - GRIPPER_COMMAND_REQ = 0x05, // Gripper command todo:add in client - - // Server → Client - GET_STATE_RESP = 0x51, //Respond to GET_STATE_REQ with FrankaArmState - GET_CONTROL_MODE_RESP = 0x52, //Respond to QUERY_STATE_REQ (1 byte: ControlMode) - SET_CONTROL_MODE_RESP = 0x53, //Respond to START_CONTROL_REQ (1 byte: status,0 = OK) - GET_SUB_PORT_RESP = 0x54, // Respond to GET_SUB_PORT_REQ (2 bytes: port number) - - // Server → Client (error) - ERROR = 0xFF // 1 byte error code - //error details + // ===== Server → Client : Request Result ===== + SUCCESS = 200, // Operation completed successfully + FAIL = 201, // Generic failure + INVALID_ARG = 202, // Invalid or out-of-range argument + BUSY = 203, // Device busy / temporary reject + UNSUPPORTED = 204, // Command not supported in current mode + TIMEOUT = 205, // Operation timed out + COMM_ERROR = 206, // Communication or CRC error + INTERNAL_ERROR = 207, // Internal logic or hardware fault + + // ===== Server → Client : Topic Publish ===== + FRANKA_ARM_STATE_PUB = 100, // Publish current robot arm state + FRANKA_GRIPPER_STATE_PUB = 101, // Publish current robot gripper state + + // ===== Client → Server : Command Topics ===== + JOINT_POSITION_CMD = 110, // Command robot joints to reach target positions + JOINT_VELOCITY_CMD = 111, // Command robot joints with velocity targets + CARTESIAN_POSE_CMD = 112, // Command robot end-effector to Cartesian pose + CARTESIAN_VELOCITY_CMD = 113, // Command robot end-effector with Cartesian velocities + JOINT_TORQUE_CMD = 114, // Command robot joints torques }; + + +//10.30todo: change according to the current protocol +// enum class MsgID : uint8_t { +// // Client → Server +// GET_STATE_REQ = 0x01, // Request a single FrankaArmState +// GET_CONTROL_MODE_REQ = 0x02, // Ask for active control mode +// SET_CONTROL_MODE_REQ = 0x03, // Switch to desired mode +// GET_SUB_PORT_REQ = 0x04, // Query PUB port number +// GRIPPER_COMMAND_REQ = 0x05, // Gripper command todo:add in client + +// // Server → Client +// GET_STATE_RESP = 0x51, //Respond to GET_STATE_REQ with FrankaArmState +// GET_CONTROL_MODE_RESP = 0x52, //Respond to QUERY_STATE_REQ (1 byte: ControlMode) +// SET_CONTROL_MODE_RESP = 0x53, //Respond to START_CONTROL_REQ (1 byte: status,0 = OK) +// GET_SUB_PORT_RESP = 0x54, // Respond to GET_SUB_PORT_REQ (2 bytes: port number) + +// // Server → Client (error) +// ERROR = 0xFF // 1 byte error code +// //error details +// }; + } // namespace protocol #endif // MSG_ID_HPP \ No newline at end of file diff --git a/include/protocol/request_result.hpp b/include/protocol/request_result.hpp new file mode 100644 index 0000000..8e048c1 --- /dev/null +++ b/include/protocol/request_result.hpp @@ -0,0 +1,70 @@ +#ifndef PROTOCOL_REQUEST_RESULT_HPP +#define PROTOCOL_REQUEST_RESULT_HPP + +#include +#include +#include + +#include "protocol/message_header.hpp" + +namespace protocol { + +// Request result codes (S -> C), values follow your table (200..207) +enum class RequestResultCode : uint8_t { + SUCCESS = 200, + FAIL = 201, + INVALID_ARG = 202, + BUSY = 203, + UNSUPPORTED = 204, + TIMEOUT = 205, + COMM_ERROR = 206, + INTERNAL_ERROR = 207, +}; + +class RequestResult { +public: + // flag indicates payload contains a detail string + static constexpr uint8_t FLAG_HAS_DETAIL = 0x01; + + RequestResult() = default; + explicit RequestResult(RequestResultCode code, std::string detail = {}) + : code_(code), detail_(std::move(detail)) {} + + RequestResultCode code() const { return code_; } + const std::string& detail() const { return detail_; } + bool ok() const { return code_ == RequestResultCode::SUCCESS; } + + // Human-readable short description for the code + static const char* description(RequestResultCode code); + + // Encode to a full message frame: 12-byte header + optional payload + // Header: + // - message_type = static_cast(code) + // - flags: bit0 indicates presence of detail string + // - payload_length: 0, or 2 + detail.size() (u16 length prefix + bytes) + // - timestamp: 0 (caller can patch if needed) + std::vector encodeMessage() const; + + // Decode from a full message frame produced by encodeMessage() + // Throws std::runtime_error on malformed input + // static RequestResult decodeMessage(const std::vector& frame); + +private: + static void write_u16_be(uint8_t* dst, uint16_t v) { + dst[0] = static_cast((v >> 8) & 0xFF); + dst[1] = static_cast(v & 0xFF); + } + static uint16_t read_u16_be(const uint8_t* src) { + return static_cast((static_cast(src[0]) << 8) | + static_cast(src[1])); + } + +private: + RequestResultCode code_ { RequestResultCode::SUCCESS }; + std::string detail_; +}; + +} // namespace protocol + +#endif // PROTOCOL_REQUEST_RESULT_HPP + diff --git a/src/FrankaProxy.cpp b/src/FrankaProxy.cpp index b7fda14..34cabcb 100644 --- a/src/FrankaProxy.cpp +++ b/src/FrankaProxy.cpp @@ -448,9 +448,9 @@ void FrankaProxy::responseSocketThread() { void FrankaProxy::handleServiceRequest(const std::vector& request, std::vector& response) { //check the request size using namespace protocol; - if (request.size() < 4) { + if (request.size() < MessageHeader::SIZE) { response = encodeErrorMessage(0x01); - std::cerr << "[FrankaProxy] Invalid request size: " << request.size() << ". Expected at least 4 bytes." << std::endl; + std::cerr << "[FrankaProxy] Invalid request size: " << request.size() << ". Expected at least 12 bytes." << std::endl; return; } @@ -462,12 +462,12 @@ void FrankaProxy::handleServiceRequest(const std::vector& request, std: // return; // } - const uint8_t* payload = data + 4; + const uint8_t* payload = data + MessageHeader::SIZE; std::vector resp; uint8_t command = payload[0]; //deal with different kinds of msg - switch (header.id) { - case static_cast (protocol::MsgID::GET_STATE_REQ): + switch (header.message_type) { + case static_cast (protocol::MsgID::GET_STATE_REQ):// { resp = encodeStateMessage(protocol::FrankaArmState::fromRobotState(getCurrentState()));//send protocol_state to pack message diff --git a/src/protocol/codec.cpp b/src/protocol/codec.cpp index ddafc7c..bde4de3 100644 --- a/src/protocol/codec.cpp +++ b/src/protocol/codec.cpp @@ -4,6 +4,7 @@ #include "protocol/franka_arm_state.hpp" #include "protocol/franka_gripper_state.hpp" #include "protocol/mode_id.hpp" +#include "protocol/request_result.hpp" #include #include #include @@ -14,31 +15,34 @@ #include namespace protocol { -// header + payload +// header + payload (12-byte header) +//payload only read std::vector encodeMessage(const MessageHeader& header, const std::vector& payload) { - std::vector result(4 + payload.size()); - header.encode(result.data()); // get head - std::memcpy(result.data() + 4, payload.data(), payload.size()); + std::vector result(MessageHeader::SIZE + payload.size()); + header.encode(result.data()); // write header + std::memcpy(result.data() + MessageHeader::SIZE, payload.data(), payload.size()); return result; } // Arm:GET_STATE_RESP/PUB_STATE std::vector encodeStateMessage(const protocol::FrankaArmState& state) { auto payload = state.encode(); // 636B - MessageHeader header{ - static_cast(MsgID::GET_STATE_RESP), - static_cast(payload.size()) - }; + MessageHeader header{}; + header.message_type = static_cast(MsgID::GET_STATE_RESP); + header.flags = 0; + header.payload_length = static_cast(payload.size()); + header.timestamp = 0; // todo:fill with actual timestamp return encodeMessage(header, payload); } //Gripper:GET_STATE_RESP/PUB_STATE std::vector encodeGripperMessage(const FrankaGripperState& gripper_state) { auto payload = gripper_state.gripper_encode(); // 23B - MessageHeader header{ - static_cast(MsgID::GET_STATE_RESP), - static_cast(payload.size()) - }; + MessageHeader header{}; + header.message_type = static_cast(MsgID::GET_STATE_RESP); + header.flags = 0; + header.payload_length = static_cast(payload.size()); + header.timestamp = 0; return encodeMessage(header, payload); } @@ -46,10 +50,11 @@ std::vector encodeGripperMessage(const FrankaGripperState& gripper_stat // QUERY_STATE_RESP std::vector encodeModeMessage(uint8_t mode_code) { std::vector payload{mode_code}; - MessageHeader header{ - static_cast(MsgID::GET_CONTROL_MODE_RESP), - static_cast(payload.size()) - }; // 1 Byte for mode code + MessageHeader header{}; + header.message_type = static_cast(MsgID::GET_CONTROL_MODE_RESP); + header.flags = 0; + header.payload_length = static_cast(payload.size()); // 1 byte + header.timestamp = 0; return encodeMessage(header, payload); } @@ -58,9 +63,11 @@ std::vector encodeModeMessage(uint8_t mode_code) { // } std::vector encodeStartControlResp(bool success, ModeID mode_id) { - MessageHeader header; - header.id = static_cast(protocol::MsgID::SET_CONTROL_MODE_RESP); - header.len = 2; + MessageHeader header{}; + header.message_type = static_cast(protocol::MsgID::SET_CONTROL_MODE_RESP); + header.flags = 0; + header.payload_length = 2; + header.timestamp = 0; std::vector payload = { static_cast(success ? 0x00 : 0x01), static_cast(mode_id) @@ -72,10 +79,11 @@ std::vector encodeStartControlResp(bool success, ModeID mode_id) { // ERROR std::vector encodeErrorMessage(uint8_t error_code) { std::vector payload{error_code}; - MessageHeader header{ - static_cast(MsgID::ERROR), - static_cast(payload.size()) - }; + MessageHeader header{}; + header.message_type = static_cast(MsgID::ERROR); + header.flags = 0; + header.payload_length = static_cast(payload.size()); + header.timestamp = 0; return encodeMessage(header, payload); } @@ -108,4 +116,9 @@ bool decodeGripperMessage(const std::vector& data, FrankaGripperState& } } -} // namespace protocol \ No newline at end of file +// Server -> Client: wrap RequestResult to message +std::vector encodeRequestResultMessage(const RequestResult& result) { + return result.encodeMessage(); +} + +} // namespace protocol diff --git a/src/protocol/message_header.cpp b/src/protocol/message_header.cpp index 0fe37fd..b91c997 100644 --- a/src/protocol/message_header.cpp +++ b/src/protocol/message_header.cpp @@ -2,18 +2,49 @@ namespace protocol { +// Encode 12-byte header in network byte order (big-endian) void MessageHeader::encode(uint8_t* buffer) const { - buffer[0] = id; - buffer[1] = (len >> 8) & 0xFF; // high byte - buffer[2] = len & 0xFF; // low byte - buffer[3] = 0; // pad is always 0 + buffer[0] = message_type; + + buffer[1] = flags; + + buffer[2] = static_cast((payload_length >> 8) & 0xFF); // high byte + buffer[3] = static_cast(payload_length & 0xFF); // low byte + + const uint64_t ts = timestamp; + buffer[4] = static_cast((ts >> 56) & 0xFF); + buffer[5] = static_cast((ts >> 48) & 0xFF); + buffer[6] = static_cast((ts >> 40) & 0xFF); + buffer[7] = static_cast((ts >> 32) & 0xFF); + buffer[8] = static_cast((ts >> 24) & 0xFF); + buffer[9] = static_cast((ts >> 16) & 0xFF); + buffer[10] = static_cast((ts >> 8) & 0xFF); + buffer[11] = static_cast(ts & 0xFF); } +// Decode 12-byte header from network byte order (big-endian) MessageHeader MessageHeader::decode(const uint8_t* buffer) { - MessageHeader header; - header.id = buffer[0]; - header.len = (buffer[1] << 8) | buffer[2]; // big-endian unpack - header.pad = buffer[3]; // always 0 + MessageHeader header{}; + header.message_type = buffer[0]; + + header.flags = buffer[1]; + + header.payload_length = static_cast( + (static_cast(buffer[2]) << 8) | + static_cast(buffer[3]) + ); + + uint64_t ts = 0; + ts |= static_cast(buffer[4]) << 56; + ts |= static_cast(buffer[5]) << 48; + ts |= static_cast(buffer[6]) << 40; + ts |= static_cast(buffer[7]) << 32; + ts |= static_cast(buffer[8]) << 24; + ts |= static_cast(buffer[9]) << 16; + ts |= static_cast(buffer[10]) << 8; + ts |= static_cast(buffer[11]); + header.timestamp = ts; + return header; } diff --git a/src/protocol/request_result.cpp b/src/protocol/request_result.cpp new file mode 100644 index 0000000..2fdee84 --- /dev/null +++ b/src/protocol/request_result.cpp @@ -0,0 +1,86 @@ +#include "protocol/request_result.hpp" + +#include +#include +#include + +namespace protocol { + +const char* RequestResult::description(RequestResultCode code) { + switch (code) { + case RequestResultCode::SUCCESS: return "Operation completed successfully"; + case RequestResultCode::FAIL: return "Generic failure"; + case RequestResultCode::INVALID_ARG: return "Request payload invalid or out of range"; + case RequestResultCode::BUSY: return "Device busy / command rejected temporarily"; + case RequestResultCode::UNSUPPORTED: return "Command not supported in current mode"; + case RequestResultCode::TIMEOUT: return "Operation timed out"; + case RequestResultCode::COMM_ERROR: return "Communication or CRC error"; + case RequestResultCode::INTERNAL_ERROR: return "Internal logic or hardware fault"; + default: return "Unknown"; + } +} + +std::vector RequestResult::encodeMessage() const { + std::vector payload; + uint8_t flags = 0; + + if (!detail_.empty()) { + if (detail_.size() > static_cast(std::numeric_limits::max() - 2)) { + throw std::runtime_error("detail too long to encode"); + } + const uint16_t len = static_cast(detail_.size()); + payload.resize(2 + len); + write_u16_be(payload.data(), len); + std::memcpy(payload.data() + 2, detail_.data(), len); + flags |= FLAG_HAS_DETAIL; + } + + MessageHeader header{}; + header.message_type = static_cast(code_); + header.flags = flags; + header.payload_length = static_cast(payload.size()); + header.timestamp = 0; + + std::vector frame(MessageHeader::SIZE + payload.size()); + header.encode(frame.data()); + if (!payload.empty()) { + std::memcpy(frame.data() + MessageHeader::SIZE, payload.data(), payload.size()); + } + return frame; +} + +// RequestResult RequestResult::decodeMessage(const std::vector& frame) { +// if (frame.size() < MessageHeader::SIZE) { +// throw std::runtime_error("frame too small"); +// } +// const uint8_t* data = frame.data(); +// const MessageHeader header = MessageHeader::decode(data); + +// const size_t expected = static_cast(MessageHeader::SIZE) + header.payload_length; +// if (frame.size() != expected) { +// throw std::runtime_error("frame size mismatch"); +// } + +// const auto code = static_cast(header.message_type); +// std::string detail; +// if (header.flags & FLAG_HAS_DETAIL) { +// if (header.payload_length < 2) { +// throw std::runtime_error("detail flag set but payload too small"); +// } +// const uint8_t* payload = data + MessageHeader::SIZE; +// const uint16_t len = read_u16_be(payload); +// if (len != static_cast(header.payload_length - 2)) { +// throw std::runtime_error("detail length mismatch"); +// } +// detail.assign(reinterpret_cast(payload + 2), len); +// } else { +// if (header.payload_length != 0) { +// throw std::runtime_error("payload present but detail flag not set"); +// } +// } + +// return RequestResult{code, std::move(detail)}; +// } + +} // namespace protocol +