DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
if (nrhs < 1) {
mexPrintf("Usage: publishLCMLog(lcm_log)\n");
return;
}
if (!lcm) lcm = lcm_create(NULL);
if (!lcm) mexErrMsgTxt("failed to create lcm node");
char* channel;
mxArray* data;
int channel_field_number = mxGetFieldNumber(prhs[0], "channel"),
data_field_number = mxGetFieldNumber(prhs[0], "data");
if (channel_field_number < 0 || data_field_number < 0)
mexErrMsgTxt(
"publishLCMLog failed: input must be a structure with fields 'channel' "
"and 'data'");
for (size_t i = 0; i < mxGetNumberOfElements(prhs[0]); i++) {
channel =
mxArrayToString(mxGetFieldByNumber(prhs[0], i, channel_field_number));
data = mxGetFieldByNumber(prhs[0], i, data_field_number);
lcm_publish(lcm, channel, mxGetData(data), mxGetNumberOfElements(data));
mxFree(channel);
}
}
int main(int argc, char** argv)
{
lcm_t* lcm = lcm_create(NULL);
srand(0);
int num_messages = 10000;
for(int i=0; i<num_messages; i++) {
int data_sz;
// if(i < num_messages / 2) {
// data_sz = rand() % 1000000;
// } else {
data_sz = 1200;
// }
char* data = (char*) calloc(1, data_sz);
snprintf(data, data_sz, "%d", i);
lcm_publish(lcm, "BUFTEST", data, 80);
printf("transmitted msg # %5d size %d\n", i, data_sz);
g_usleep(1000);
free(data);
}
return 0;
}
static int
do_echo_test(void)
{
int maxlen = 10000;
int minlen = 10;
g_echo_data = malloc(maxlen);
lcm_subscription_t* subs = lcm_subscribe(g_lcm, "TEST_ECHO_REPLY", echo_handler, NULL);
g_echo_response_count = 0;
int iter;
for(iter=0; iter<100; iter++)
{
g_echo_msg_len = rand() % (maxlen - minlen) + minlen;
int i;
for(i=0; i<g_echo_msg_len; i++)
g_echo_data[i] = rand() % 256;
lcm_publish(g_lcm, "TEST_ECHO", g_echo_data, g_echo_msg_len);
if(!_lcm_handle_timeout(g_lcm, 500) || (g_echo_response_count != iter+1))
{
info("echo test failed to receive response on iteration %d\n", iter);
free(g_echo_data);
return 0;
}
}
info("%-32s : PASSED\n", "echo test");
lcm_unsubscribe(g_lcm, subs);
free(g_echo_data);
return 1;
}
static void
on_unpause_mi_activate (GtkMenuItem *menu_item, void *user_data)
{
RendererSimTraffic *self = user_data;
char msg[40];
sprintf (msg, "%"PRId64, self->menu_car_id);
lcm_publish (self->lc, "TSIM_UNPAUSE", (uint8_t*)msg, strlen (msg));
}
inline int
LCM::publish(const std::string& channel, void *data, unsigned int datalen) {
if(!this->lcm) {
fprintf(stderr,
"LCM instance not initialized. Ignoring call to publish()\n");
return -1;
}
return lcm_publish(this->lcm, channel.c_str(), data, datalen);
}
void handler(const lcm_recv_buf_t *rbuf, const char *channel, void *u)
{
logplayer_t *l = (logplayer_t *) u;
if (l->verbose)
printf("%.3f Channel %-20s size %d\n", rbuf->recv_utime / 1000000.0, channel,
rbuf->data_size);
lcm_publish(l->lcm_out, channel, rbuf->data, rbuf->data_size);
}
int LcmTunnel::publishLcmMessagesInBuf(int numBytes)
{
uint32_t msgOffset = 0;
while (msgOffset < numBytes) {
//decode
lcm_tunnel_sub_msg_t p;
msgOffset += lcm_tunnel_sub_msg_t_decode(buf, msgOffset, numBytes - msgOffset, &p);
// and publish
LcmTunnelServer::check_and_send_to_tunnels(p.channel, p.data, p.data_size, this);
lcm_publish(lcm, p.channel, p.data, p.data_size);
if (verbose)
printf("publishing [%s] (%.3fKb)\n", p.channel, p.data_size * 1e-3);
check_ret(lcm_tunnel_sub_msg_t_decode_cleanup(&p));
}
assert(msgOffset==numBytes);
}
/**
* Publishes a message.
*
* @see lcm_publish
*
* @pre The Lua arguments on the stack:
* A LCM userdata (self), a string containing the channel, and a string
* containing message data (from an encode).
*
* @post The Lua return values on the stack:
* Nothing.
*
* @param L The Lua state.
* @return The number of return values on the Lua stack.
*
* @throws Lua error if the message cannot be published.
*/
static int impl_lcm_publish(lua_State * L) {
/* we expect 3 arguments */
lua_settop(L, 3);
/* get the lcm userdata */
impl_lcm_userdata_t * lcmu = impl_lcm_checkuserdata(L, 1);
/* get the channel */
const char * channel = luaL_checkstring(L, 2);
/* get the buffer */
size_t data_size;
const char * data = luaL_checklstring(L, 3, &data_size);
/* publish the message */
if(lcm_publish(lcmu->lcm, channel, data, data_size) != 0) {
lua_pushstring(L, "error lcm publish");
lua_error(L);
}
return 0;
}
int LcmTunnel::on_tcp_data(GIOChannel * source, GIOCondition cond, void *user_data)
{
int ret = TRUE;
LcmTunnel * self = (LcmTunnel*) user_data;
// increase buffer size if needed
if (self->buf_sz < self->bytes_to_read) {
assert(self->bytes_read == 0);
self->buf = (char *) realloc(self->buf, self->bytes_to_read);
self->buf_sz = self->bytes_to_read;
}
ssize_t nread = read(ssocket_get_fd(self->tcp_sock), self->buf + self->bytes_read, self->bytes_to_read
- self->bytes_read);
if (nread <= 0) {
perror("tcp receive error: ");
LcmTunnelServer::disconnectClient(self);
return FALSE;
}
self->bytes_read += nread;
assert(self->bytes_read <= self->bytes_to_read);
if (self->bytes_read != self->bytes_to_read)
return TRUE;
switch (self->tunnel_state) {
case CLIENT_MSG_SZ:
self->bytes_to_read = ntohl(*(uint32_t*) self->buf);
self->tunnel_state = CLIENT_MSG_DATA;
break;
case CLIENT_MSG_DATA:
{
lcm_tunnel_params_t tp_rec;
int decode_status = lcm_tunnel_params_t_decode(self->buf, 0, self->bytes_read, &tp_rec);
if (decode_status <= 0) {
fprintf(stdout, "invalid request (%d)\n", decode_status);
return FALSE;
}
self->tunnel_params = lcm_tunnel_params_t_copy(&tp_rec);
if (self->udp_fd >= 0) {
close(self->udp_fd);
}
self->udp_fd = -1;
if (self->tunnel_params->udp) {
//setup our UDP socket, and send info to client
struct sockaddr_in client_addr;
socklen_t addrlen = sizeof(client_addr);
getpeername(self->tcp_sock->socket, (struct sockaddr*) &client_addr, &addrlen);
self->server_udp_port = ntohs(client_addr.sin_port);
client_addr.sin_port = htons(self->tunnel_params->udp_port);
// allocate UDP socket
self->udp_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (self->udp_fd < 0) {
perror("allocating UDP socket");
LcmTunnelServer::disconnectClient(self);
return FALSE;
}
connect(self->udp_fd, (struct sockaddr*) &client_addr, sizeof(client_addr));
// transmit the udp port info
struct sockaddr_in udp_addr;
socklen_t udp_addr_len = sizeof(udp_addr);
memset(&udp_addr, 0, sizeof(udp_addr));
udp_addr.sin_family = AF_INET;
udp_addr.sin_addr.s_addr = INADDR_ANY;
udp_addr.sin_port = 0;
getsockname(self->udp_fd, (struct sockaddr*) &udp_addr, &udp_addr_len);
lcm_tunnel_params_t tp_port_msg;
tp_port_msg.channels = (char *) " ";
tp_port_msg.udp_port = ntohs(udp_addr.sin_port);
int msg_sz = lcm_tunnel_params_t_encoded_size(&tp_port_msg);
uint8_t msg[msg_sz];
lcm_tunnel_params_t_encode(msg, 0, msg_sz, &tp_port_msg);
uint32_t msg_sz_n = htonl(msg_sz);
if (4 != _fileutils_write_fully(ssocket_get_fd(self->tcp_sock), &msg_sz_n, 4)) {
perror("sending subscription data");
LcmTunnelServer::disconnectClient(self);
return FALSE;
}
if (msg_sz != _fileutils_write_fully(ssocket_get_fd(self->tcp_sock), msg, msg_sz)) {
perror("sending subscription data");
LcmTunnelServer::disconnectClient(self);
return FALSE;
}
self->udp_ioc = g_io_channel_unix_new(self->udp_fd);
self->udp_sid = g_io_add_watch(self->udp_ioc, G_IO_IN, LcmTunnel::on_udp_data, self);
//we're done setting up the UDP connection...Disconnect tcp socket
self->closeTCPSocket();
ret = false;
}
//get ready to receive
self->tunnel_state = RECV_CHAN_SZ;
self->bytes_to_read = 4;
// if (self->server_params->verbose)
fprintf(stderr, "%s subscribed to \"%s\" -- ", self->name, self->tunnel_params->channels);
if (self->udp_fd >= 0) {
if (self->tunnel_params->fec > 1)
fprintf(stderr, "UDP with FEC rate of %.2f and max_delay of %dms\n", self->tunnel_params->fec,
self->tunnel_params->max_delay_ms);
else if (self->tunnel_params->fec < -1)
fprintf(stderr, "UDP with DUP rate of %d and max_delay of %dms\n", (int) -self->tunnel_params->fec,
self->tunnel_params->max_delay_ms);
else
fprintf(stderr, "UDP with a max_delay of %dms\n", self->tunnel_params->max_delay_ms);
}
else {
fprintf(stderr, "TCP with max_delay of %dms and tcp_max_age_ms of %d\n", self->tunnel_params->max_delay_ms,
self->tunnel_params->tcp_max_age_ms);
}
self->init_regex(self->tunnel_params->channels);
//subscribe to the LCM channels
if (self->subscription) {
lcm_unsubscribe(self->lcm, self->subscription);
}
self->subscription = lcm_subscribe(self->lcm, self->tunnel_params->channels, on_lcm_message, self);
}
break;
case SERVER_MSG_SZ:
self->bytes_to_read = ntohl(*(uint32_t*) self->buf);
self->tunnel_state = SERVER_MSG_DATA;
break;
case SERVER_MSG_DATA:
{
lcm_tunnel_params_t tp_rec;
int decode_status = lcm_tunnel_params_t_decode(self->buf, 0, self->bytes_read, &tp_rec);
if (decode_status <= 0) {
fprintf(stderr, "invalid request (%d)\n", decode_status);
return FALSE;
}
assert(self->udp_fd>0);
struct sockaddr_in client_addr;
socklen_t addrlen = sizeof(client_addr);
getpeername(self->tcp_sock->socket, (struct sockaddr*) &client_addr, &addrlen);
self->server_udp_port = tp_rec.udp_port;
client_addr.sin_port = htons(tp_rec.udp_port);
//connect the udp socket
connect(self->udp_fd, (struct sockaddr*) &client_addr, sizeof(client_addr));
//now we can subscribe to LCM
fprintf(stderr, "%s subscribed to \"%s\" \n", self->name, self->tunnel_params->channels);
self->subscription = lcm_subscribe(self->lcm, self->tunnel_params->channels, on_lcm_message, self);
//we're done setting up the UDP connection...Disconnect tcp socket
self->closeTCPSocket();
ret = FALSE; //don't want the TCP handler to be run again
}
break;
case RECV_CHAN_SZ:
self->bytes_to_read = ntohl(*(uint32_t*) self->buf);
self->tunnel_state = RECV_CHAN;
if (self->channel_sz < self->bytes_to_read + 1) {
self->channel = (char *) realloc(self->channel, self->bytes_to_read + 1);
self->channel_sz = self->bytes_to_read + 1;
}
break;
case RECV_CHAN:
memcpy(self->channel, self->buf, self->bytes_read);
self->channel[self->bytes_read] = 0;
self->bytes_to_read = 4;
self->tunnel_state = RECV_DATA_SZ;
break;
case RECV_DATA_SZ:
self->bytes_to_read = ntohl(*(uint32_t*) self->buf);
self->tunnel_state = RECV_DATA;
break;
case RECV_DATA:
if (self->verbose)
printf("Recieved TCP message on channel \"%s\"\n", self->channel);
LcmTunnelServer::check_and_send_to_tunnels(self->channel, self->buf, self->bytes_read, self);
lcm_publish(self->lcm, self->channel, (uint8_t*) self->buf, self->bytes_read);
self->bytes_to_read = 4;
self->tunnel_state = RECV_CHAN_SZ;
break;
}
self->bytes_read = 0;
return ret;
}
static void
on_add_bt_clicked (GtkWidget *bt, void *user_data)
{
RendererSimTraffic *self = user_data;
lcm_publish (self->lc, "TSIM_NEWCAR", NULL, 0);
}
