Skip to content

some reconstruction accorting to protocol #4

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
ZhuoyueLi wants to merge 1 commit into from
Closed

some reconstruction accorting to protocol #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 15 additions & 11 deletions include/FrankaProxy.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
23 changes: 19 additions & 4 deletions include/protocol/codec.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@

namespace protocol {

class RequestResult; // forward declaration?

// encode std::array<double, N> (fixed size)
template <size_t N>
inline void encode_array_f64(uint8_t*& ptr, const std::array<double, N>& in) {
Expand Down Expand Up @@ -117,14 +119,27 @@ inline bool decode_bool(const uint8_t*& ptr) {
std::vector<uint8_t> encodeMessage(const MessageHeader& header, const std::vector<uint8_t>& payload);
std::vector<uint8_t> encodeStateMessage(const FrankaArmState& state);
std::vector<uint8_t> encodeModeMessage(uint8_t mode_code);
std::vector<uint8_t> encodeErrorMessage(uint8_t error_code);
std::vector<uint8_t> encodeGripperMessage(const FrankaGripperState& gripper_state);
//Todo:check if need vector of following 2 functions
std::vector<uint8_t> encodeRequestResultMessage(const RequestResult& result); // Todo: implement RequestResult class
std::vector<uint8_t> encodePubPortMessage(uint16_t Pubport);//Todo:implement, and whta Pubport old name
//Todo:check if return bool
bool decodeModeMessage(const std::vector<uint8_t>& data, uint8_t& mode_code);//Todo:change into FrankaArmControl
bool decodeCommandMessage(const std::vector<uint8_t>& 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<uint8_t>& data, FrankaArmState& arm_state);
// std::vector<uint8_t> encodeErrorMessage(uint8_t error_code);

std::vector<uint8_t> encodeGripperMessage(const FrankaGripperState& gripper_state);
bool decodeGripperMessage(const std::vector<uint8_t>& data, FrankaGripperState& gripper_state);
std::vector<uint8_t> encodeStartControlResp(bool success, ModeID mode_id);
// std::vector<uint8_t> encodeStartControlResp(bool success, ModeID mode_id);




} // namespace protocol
#endif // CODEC_HPP
#endif // CODEC_HPP
29 changes: 22 additions & 7 deletions include/protocol/message_header.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,20 +4,35 @@
#include <cstddef>
namespace protocol {

#include <cstdint>

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
48 changes: 37 additions & 11 deletions include/protocol/mode_id.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,24 +5,50 @@

namespace protocol {

#include <cstdint>
#include <string>

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
69 changes: 52 additions & 17 deletions include/protocol/msg_id.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,25 +3,60 @@
#include <cstdint>

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
70 changes: 70 additions & 0 deletions include/protocol/request_result.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
#ifndef PROTOCOL_REQUEST_RESULT_HPP
#define PROTOCOL_REQUEST_RESULT_HPP

#include <cstdint>
#include <string>
#include <vector>

#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<uint8_t>(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<uint8_t> encodeMessage() const;

// Decode from a full message frame produced by encodeMessage()
// Throws std::runtime_error on malformed input
// static RequestResult decodeMessage(const std::vector<uint8_t>& frame);

private:
static void write_u16_be(uint8_t* dst, uint16_t v) {
dst[0] = static_cast<uint8_t>((v >> 8) & 0xFF);
dst[1] = static_cast<uint8_t>(v & 0xFF);
}
static uint16_t read_u16_be(const uint8_t* src) {
return static_cast<uint16_t>((static_cast<uint16_t>(src[0]) << 8) |
static_cast<uint16_t>(src[1]));
}

private:
RequestResultCode code_ { RequestResultCode::SUCCESS };
std::string detail_;
};

} // namespace protocol

#endif // PROTOCOL_REQUEST_RESULT_HPP

10 changes: 5 additions & 5 deletions src/FrankaProxy.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -448,9 +448,9 @@ void FrankaProxy::responseSocketThread() {
void FrankaProxy::handleServiceRequest(const std::vector<uint8_t>& request, std::vector<uint8_t>& 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;
}

Expand All @@ -462,12 +462,12 @@ void FrankaProxy::handleServiceRequest(const std::vector<uint8_t>& request, std:
// return;
// }

const uint8_t* payload = data + 4;
const uint8_t* payload = data + MessageHeader::SIZE;
std::vector<uint8_t> resp;
uint8_t command = payload[0];
//deal with different kinds of msg
switch (header.id) {
case static_cast<int> (protocol::MsgID::GET_STATE_REQ):
switch (header.message_type) {
case static_cast<int> (protocol::MsgID::GET_STATE_REQ)://
{

resp = encodeStateMessage(protocol::FrankaArmState::fromRobotState(getCurrentState()));//send protocol_state to pack message
Expand Down
Loading
Loading