limingjie/OmniSocketGo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 基于Python ROS2的控制程序

Browse Source
This commit is contained in:
Mock
2026-04-03 20:00:33 +08:00
parent 6ece408d9f
commit 9ffc36f50d
26 changed files with 2193 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

326
python/omnisocket/_omnisocket.c
View File

@@ -8,6 +8,11 @@ typedef struct PyOmniSession {
omnisocket_session_t session;
} PyOmniSession;
typedef struct PyOmniUdpSession {
PyObject_HEAD
omnisocket_udp_session_t session;
} PyOmniUdpSession;
PyDoc_STRVAR(
PyOmniSession_recv_doc,
"recv(timeout_ms=-1) -> (from_peer, msg_type, payload) | None"
@@ -22,6 +27,66 @@ PyDoc_STRVAR(
"current frame has already been consumed and is lost."
);
static PyObject *build_recv_result(const message_t *msg) {
PyObject *body = NULL;
PyObject *result = NULL;
body = PyBytes_FromStringAndSize((const char *) msg->body, (Py_ssize_t) msg->body_len);
if (body == NULL) {
return NULL;
}
result = Py_BuildValue("(siO)", msg->from, (int) msg->type, body);
Py_DECREF(body);
return result;
}
static PyObject *build_recv_meta_dict(
const char *from_peer,
const char *to_peer,
const char *file_name,
int msg_type,
unsigned long long message_id,
unsigned long long body_len
) {
return Py_BuildValue(
"{s:s,s:s,s:s,s:i,s:K,s:K}",
"from",
from_peer,
"to",
to_peer,
"file_name",
file_name,
"msg_type",
msg_type,
"message_id",
message_id,
"body_len",
body_len
);
}
static PyObject *build_stats_dict(const omnisocket_session_stats_t *stats) {
return Py_BuildValue(
"{s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:i}",
"send_calls",
(unsigned long long) stats->send_calls,
"send_bytes",
(unsigned long long) stats->send_bytes,
"send_errors",
(unsigned long long) stats->send_errors,
"recv_calls",
(unsigned long long) stats->recv_calls,
"recv_bytes",
(unsigned long long) stats->recv_bytes,
"recv_timeouts",
(unsigned long long) stats->recv_timeouts,
"recv_errors",
(unsigned long long) stats->recv_errors,
"connected",
stats->connected
);
}
static PyObject *PyOmniSession_new(PyTypeObject *type, PyObject *args, PyObject *kwargs) {
PyOmniSession *self;
(void) args;
@@ -165,7 +230,6 @@ static PyObject *PyOmniSession_recv(PyOmniSession *self, PyObject *args, PyObjec
int timeout_ms = -1;
int rc;
message_t msg;
PyObject *body = NULL;
PyObject *result = NULL;
static char *kwlist[] = {"timeout_ms", NULL};
@@ -187,13 +251,7 @@ static PyObject *PyOmniSession_recv(PyOmniSession *self, PyObject *args, PyObjec
return PyErr_SetFromErrno(PyExc_OSError);
}
body = PyBytes_FromStringAndSize((const char *) msg.body, (Py_ssize_t) msg.body_len);
if (body == NULL) {
protocol_message_clear(&msg);
return NULL;
}
result = Py_BuildValue("(siO)", msg.from, (int) msg.type, body);
Py_DECREF(body);
result = build_recv_result(&msg);
protocol_message_clear(&msg);
return result;
}
@@ -237,19 +295,12 @@ static PyObject *PyOmniSession_recv_into(PyOmniSession *self, PyObject *args, Py
return PyErr_SetFromErrno(PyExc_OSError);
}
result = Py_BuildValue(
"{s:s,s:s,s:s,s:i,s:K,s:K}",
"from",
result = build_recv_meta_dict(
meta.from,
"to",
meta.to,
"file_name",
meta.file_name,
"msg_type",
(int) meta.type,
"message_id",
(unsigned long long) meta.id,
"body_len",
(unsigned long long) meta.body_len
);
return result;
@@ -260,25 +311,7 @@ static PyObject *PyOmniSession_stats(PyOmniSession *self, PyObject *Py_UNUSED(ig
memset(&stats, 0, sizeof(stats));
omnisocket_session_stats_snapshot(&self->session, &stats);
return Py_BuildValue(
"{s:K,s:K,s:K,s:K,s:K,s:K,s:K,s:i}",
"send_calls",
(unsigned long long) stats.send_calls,
"send_bytes",
(unsigned long long) stats.send_bytes,
"send_errors",
(unsigned long long) stats.send_errors,
"recv_calls",
(unsigned long long) stats.recv_calls,
"recv_bytes",
(unsigned long long) stats.recv_bytes,
"recv_timeouts",
(unsigned long long) stats.recv_timeouts,
"recv_errors",
(unsigned long long) stats.recv_errors,
"connected",
stats.connected
);
return build_stats_dict(&stats);
}
static PyMethodDef PyOmniSession_methods[] = {
@@ -295,6 +328,211 @@ static PyTypeObject PyOmniSessionType = {
PyVarObject_HEAD_INIT(NULL, 0)
};
static PyObject *PyOmniUdpSession_new(PyTypeObject *type, PyObject *args, PyObject *kwargs) {
PyOmniUdpSession *self;
(void) args;
(void) kwargs;
self = (PyOmniUdpSession *) type->tp_alloc(type, 0);
if (self == NULL) {
return NULL;
}
if (omnisocket_udp_session_init(&self->session) != 0) {
type->tp_free((PyObject *) self);
return PyErr_SetFromErrno(PyExc_OSError);
}
return (PyObject *) self;
}
static void PyOmniUdpSession_dealloc(PyOmniUdpSession *self) {
omnisocket_udp_session_destroy(&self->session);
Py_TYPE(self)->tp_free((PyObject *) self);
}
static PyObject *PyOmniUdpSession_connect(PyOmniUdpSession *self, PyObject *args, PyObject *kwargs) {
const char *server_addr;
const char *peer_id;
const char *bind_ip = "";
const char *bind_device = "";
int enable_timestamping = 0;
int rc;
static char *kwlist[] = {
"server_addr",
"peer_id",
"bind_ip",
"bind_device",
"enable_timestamping",
NULL
};
if (!PyArg_ParseTupleAndKeywords(
args,
kwargs,
"ss|ssi",
kwlist,
&server_addr,
&peer_id,
&bind_ip,
&bind_device,
&enable_timestamping)) {
return NULL;
}
Py_BEGIN_ALLOW_THREADS
rc = omnisocket_udp_session_connect(
&self->session,
server_addr,
peer_id,
bind_ip,
bind_device,
enable_timestamping
);
Py_END_ALLOW_THREADS
if (rc != 0) {
return PyErr_SetFromErrno(PyExc_OSError);
}
Py_RETURN_NONE;
}
static PyObject *PyOmniUdpSession_close(PyOmniUdpSession *self, PyObject *Py_UNUSED(ignored)) {
int rc;
Py_BEGIN_ALLOW_THREADS
rc = omnisocket_udp_session_close(&self->session);
Py_END_ALLOW_THREADS
if (rc != 0) {
return PyErr_SetFromErrno(PyExc_OSError);
}
Py_RETURN_NONE;
}
static PyObject *PyOmniUdpSession_send(PyOmniUdpSession *self, PyObject *args, PyObject *kwargs) {
const char *to;
Py_buffer payload;
int rc;
static char *kwlist[] = {"to", "data", NULL};
memset(&payload, 0, sizeof(payload));
if (!PyArg_ParseTupleAndKeywords(args, kwargs, "sy*", kwlist, &to, &payload)) {
return NULL;
}
Py_BEGIN_ALLOW_THREADS
rc = omnisocket_udp_session_send(&self->session, to, payload.buf, (size_t) payload.len);
Py_END_ALLOW_THREADS
PyBuffer_Release(&payload);
if (rc != 0) {
return PyErr_SetFromErrno(PyExc_OSError);
}
Py_RETURN_NONE;
}
static PyObject *PyOmniUdpSession_recv(PyOmniUdpSession *self, PyObject *args, PyObject *kwargs) {
int timeout_ms = -1;
int rc;
message_t msg;
PyObject *result = NULL;
static char *kwlist[] = {"timeout_ms", NULL};
if (!PyArg_ParseTupleAndKeywords(args, kwargs, "|i", kwlist, &timeout_ms)) {
return NULL;
}
protocol_message_init(&msg);
Py_BEGIN_ALLOW_THREADS
rc = omnisocket_udp_session_recv(&self->session, &msg, timeout_ms);
Py_END_ALLOW_THREADS
if (rc == 1) {
protocol_message_clear(&msg);
Py_RETURN_NONE;
}
if (rc != 0) {
protocol_message_clear(&msg);
return PyErr_SetFromErrno(PyExc_OSError);
}
result = build_recv_result(&msg);
protocol_message_clear(&msg);
return result;
}
static PyObject *PyOmniUdpSession_recv_into(PyOmniUdpSession *self, PyObject *args, PyObject *kwargs) {
PyObject *buffer_obj;
Py_buffer view;
int timeout_ms = -1;
int rc;
udp_client_recv_meta_t meta;
PyObject *result = NULL;
static char *kwlist[] = {"buffer", "timeout_ms", NULL};
memset(&view, 0, sizeof(view));
memset(&meta, 0, sizeof(meta));
if (!PyArg_ParseTupleAndKeywords(args, kwargs, "O|i", kwlist, &buffer_obj, &timeout_ms)) {
return NULL;
}
if (PyObject_GetBuffer(buffer_obj, &view, PyBUF_WRITABLE) != 0) {
return NULL;
}
Py_BEGIN_ALLOW_THREADS
rc = omnisocket_udp_session_recv_into(&self->session, view.buf, (size_t) view.len, &meta, timeout_ms);
Py_END_ALLOW_THREADS
PyBuffer_Release(&view);
if (rc == 1) {
Py_RETURN_NONE;
}
if (rc == 2) {
PyErr_Format(
PyExc_BufferError,
"buffer too small: need %zu bytes; current frame was already consumed and dropped",
meta.body_len
);
return NULL;
}
if (rc != 0) {
return PyErr_SetFromErrno(PyExc_OSError);
}
result = build_recv_meta_dict(
meta.from,
meta.to,
meta.file_name,
(int) meta.type,
(unsigned long long) meta.id,
(unsigned long long) meta.body_len
);
return result;
}
static PyObject *PyOmniUdpSession_stats(PyOmniUdpSession *self, PyObject *Py_UNUSED(ignored)) {
omnisocket_session_stats_t stats;
memset(&stats, 0, sizeof(stats));
omnisocket_udp_session_stats_snapshot(&self->session, &stats);
return build_stats_dict(&stats);
}
static PyMethodDef PyOmniUdpSession_methods[] = {
{"connect", (PyCFunction) PyOmniUdpSession_connect, METH_VARARGS | METH_KEYWORDS, NULL},
{"close", (PyCFunction) PyOmniUdpSession_close, METH_NOARGS, NULL},
{"send", (PyCFunction) PyOmniUdpSession_send, METH_VARARGS | METH_KEYWORDS, NULL},
{"recv", (PyCFunction) PyOmniUdpSession_recv, METH_VARARGS | METH_KEYWORDS, PyOmniSession_recv_doc},
{"recv_into", (PyCFunction) PyOmniUdpSession_recv_into, METH_VARARGS | METH_KEYWORDS, PyOmniSession_recv_into_doc},
{"stats", (PyCFunction) PyOmniUdpSession_stats, METH_NOARGS, NULL},
{NULL, NULL, 0, NULL}
};
static PyTypeObject PyOmniUdpSessionType = {
PyVarObject_HEAD_INIT(NULL, 0)
};
static PyModuleDef omnisocket_module = {
PyModuleDef_HEAD_INIT,
.m_name = "_omnisocket",
@@ -315,6 +553,17 @@ PyMODINIT_FUNC PyInit__omnisocket(void) {
return NULL;
}
PyOmniUdpSessionType.tp_name = "omnisocket.UdpSession";
PyOmniUdpSessionType.tp_basicsize = sizeof(PyOmniUdpSession);
PyOmniUdpSessionType.tp_flags = Py_TPFLAGS_DEFAULT;
PyOmniUdpSessionType.tp_new = PyOmniUdpSession_new;
PyOmniUdpSessionType.tp_dealloc = (destructor) PyOmniUdpSession_dealloc;
PyOmniUdpSessionType.tp_methods = PyOmniUdpSession_methods;
if (PyType_Ready(&PyOmniUdpSessionType) < 0) {
return NULL;
}
module = PyModule_Create(&omnisocket_module);
if (module == NULL) {
return NULL;
@@ -327,6 +576,13 @@ PyMODINIT_FUNC PyInit__omnisocket(void) {
return NULL;
}
Py_INCREF(&PyOmniUdpSessionType);
if (PyModule_AddObject(module, "UdpSession", (PyObject *) &PyOmniUdpSessionType) != 0) {
Py_DECREF(&PyOmniUdpSessionType);
Py_DECREF(module);
return NULL;
}
if (PyModule_AddIntConstant(module, "MSG_TYPE_TEXT", MSG_TYPE_TEXT) != 0 ||
PyModule_AddIntConstant(module, "MSG_TYPE_FILE", MSG_TYPE_FILE) != 0 ||
PyModule_AddIntConstant(module, "MSG_TYPE_REGISTER", MSG_TYPE_REGISTER) != 0 ||