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;
}
/**
* Unsubscribes from a channel. Removes the subscription from the internal
* subscription table.
*
* @see lcm_unsubscribe
*
* @pre The Lua arguments on the stack:
* A LCM userdata (self), and a subscription reference number.
*
* @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 subscription cannot be unsubscribed.
*/
static int impl_lcm_unsubscribe(lua_State * L) {
/* we expect 2 arguments */
lua_settop(L, 2);
/* get the lcm userdata */
impl_lcm_userdata_t * lcmu = impl_lcm_checkuserdata(L, 1);
/* get the ref_num */
int ref_num = luaL_checkint(L, 2);
/* get subscription table entry, this pushes the handler on the stack */
lcm_subscription_t * subscription;
if(!impl_lcm_removefromsubscriptiontable(L, 1, ref_num, &subscription)) {
/* made up reference number */
lua_pushstring(L, "subscription number invalid");
lua_error(L);
}
/* unsubscribe */
if(lcm_unsubscribe(lcmu->lcm, subscription) != 0) {
lua_pushstring(L, "error lcm unsubscribe");
lua_error(L);
}
return 0;
}
static int
udpm_self_test (lcm_udpm_t *lcm)
{
int success = 0;
int status;
// register a handler for the self test message
lcm_subscription_t *h = lcm_subscribe (lcm->lcm, SELF_TEST_CHANNEL,
self_test_handler, &success);
// transmit a message
char *msg = "lcm self test";
lcm_udpm_publish (lcm, SELF_TEST_CHANNEL, (uint8_t*)msg, strlen (msg));
// wait one second for message to be received
GTimeVal now, endtime;
g_get_current_time(&now);
endtime.tv_sec = now.tv_sec + 10;
endtime.tv_usec = now.tv_usec;
// periodically retransmit, just in case
GTimeVal retransmit_interval = { 0, 100000 };
GTimeVal next_retransmit;
lcm_timeval_add (&now, &retransmit_interval, &next_retransmit);
int recvfd = lcm->notify_pipe[0];
do {
GTimeVal selectto;
lcm_timeval_subtract (&next_retransmit, &now, &selectto);
fd_set readfds;
FD_ZERO (&readfds);
FD_SET (recvfd,&readfds);
g_get_current_time(&now);
if (lcm_timeval_compare (&now, &next_retransmit) > 0) {
status = lcm_udpm_publish (lcm, SELF_TEST_CHANNEL, (uint8_t*)msg,
strlen (msg));
lcm_timeval_add (&now, &retransmit_interval, &next_retransmit);
}
status=select (recvfd + 1,&readfds,0,0, (struct timeval*) &selectto);
if (status > 0 && FD_ISSET (recvfd,&readfds)) {
lcm_udpm_handle (lcm);
}
g_get_current_time(&now);
} while (! success && lcm_timeval_compare (&now, &endtime) < 0);
lcm_unsubscribe (lcm->lcm, h);
dbg (DBG_LCM, "LCM: self test complete\n");
// if the self test message was received, then the handler modified the
// value of success to be 1
return (success == 1)?0:-1;
}
LcmTunnel::~LcmTunnel()
{
if (subscription) {
lcm_unsubscribe(lcm, subscription);
}
//cleanup the sending thread state
g_mutex_lock(sendQueueLock);
stopSendThread = true;
g_cond_broadcast(sendQueueCond);
g_mutex_unlock(sendQueueLock);
g_thread_join(sendThread); //wait for thread to exit
g_mutex_lock(sendQueueLock);
while (!sendQueue.empty()) {
delete sendQueue.front();
sendQueue.pop_front();
}
g_mutex_unlock(sendQueueLock);
g_mutex_free(sendQueueLock);
g_cond_free(sendQueueCond);
if (udp_fd >= 0) {
//send out a disconnect message
lcm_tunnel_disconnect_msg_t disc_msg;
disc_msg.utime = _timestamp_now();
int msg_sz = lcm_tunnel_disconnect_msg_t_encoded_size(&disc_msg);
uint8_t msg_buf[msg_sz];
lcm_tunnel_disconnect_msg_t_encode(msg_buf, 0, msg_sz, &disc_msg);
send(udp_fd, msg_buf, msg_sz, 0);
//close UDP socket
close(udp_fd);
g_io_channel_unref(udp_ioc);
g_source_remove(udp_sid);
}
//close TCP socket
closeTCPSocket();
if (regex != NULL)
g_regex_unref(regex);
free(buf);
free(channel);
free(recFlags);
free(tunnel_params);
if (ldpc_dec != NULL)
delete ldpc_dec;
}
/**
* Cleans up the LCM userdata. This is the __gc metamethod of the LCM userdata.
* This method is called automatically by the Lua garbage collector.
*
* Automatically unsubscribes all channels.
*
* @see lcm_destroy
*
* @pre The Lua arguments on the stack:
* A LCM userdata (self).
*
* @post The Lua return values on the stack:
* Nothing.
*
* @param L The Lua state.
* @return The number of return values on the Lua stack.
*/
static int impl_lcm_gc(lua_State * L) {
/* we expect 1 argument */
lua_settop(L, 1);
/* get the lcm userdata */
impl_lcm_userdata_t * lcmu = impl_lcm_checkuserdata(L, 1);
/* check if this userdata was ever created */
if(!lcmu->lcm) {
return 0;
}
/* get subscription table */
impl_lcm_getsubscriptiontable(L, 1);
/* subscription table traversal */
lua_pushnil(L); /* first key */
while(lua_next(L, 2) != 0) {
/* get the subscription userdata */
lua_pushstring(L, "userdata");
lua_rawget(L, -2);
impl_sub_userdata_t * subu =
(impl_sub_userdata_t *) lua_touserdata(L, -1);
/* unsubscribe */
if(lcm_unsubscribe(lcmu->lcm, subu->subscription) != 0) {
lua_pushstring(L, "error lcm unsubscribe");
lua_error(L);
}
/* pop the userdata and subscription table entry */
lua_pop(L, 2);
}
/* free lcm */
lcm_destroy(lcmu->lcm);
/* clear out the subscription table */
impl_lcm_createsubscriptiontable(L, 1);
return 0;
}
inline void
LCM::unsubscribe(Subscription *subscription) {
if(!this->lcm) {
fprintf(stderr,
"LCM instance not initialized. Ignoring call to unsubscribe()\n");
return;
}
std::vector<Subscription*>::iterator iter;
std::vector<Subscription*>::iterator eiter = subscriptions.end();
for(iter=subscriptions.begin(); iter!= eiter; ++iter) {
if(*iter == subscription) {
lcm_unsubscribe(lcm, subscription->c_subs);
subscriptions.erase(iter);
delete subscription;
break;
}
}
}
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;
}
