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;
}
/**
* Subscribes to a channel. Requires the user to specify a Lua handler (which
* is called by generic_lcm_handler). The whole process is somewhat involved.
*
* In Lua 5.1, the handlers are stored in the LCM userdata's environment.
*
* Subscriptions are managed by the LCM userdata, and the user is not given
* access to the subscription userdata. This subscribe method returns
* subscription reference number, which is used to unsubscribe later. If the
* user does not manually unsubscribe, the subscription will automatically
* terminate during garbage collection of the LCM userdata.
*
* @see lcm_subscribe
*
* @pre The Lua arguments on the stack:
* A LCM userdata (self), a string containing the channel, and a function
* for the handler.
*
* @post The Lua return values on the stack:
* A subscription reference number.
*
* @param L The Lua state.
* @return The number of return values on the Lua stack.
*/
static int impl_lcm_subscribe(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);
/* check the handler */
luaL_checktype(L, 3, LUA_TFUNCTION);
/* add a subscription table entry */
int * ref_num_ptr;
lcm_subscription_t ** subscription;
void * userdata =
impl_lcm_addtosubscriptiontable(L, 1, &ref_num_ptr, &subscription);
/* pop subscription table */
lua_pop(L, 1);
/* do the actual subscribe */
*subscription =
lcm_subscribe(lcmu->lcm, channel, impl_lcm_c_handler, userdata);
/* push reference number */
lua_pushinteger(L, *ref_num_ptr);
return 1;
}
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;
}
Subscription*
LCM::subscribeFunction(const std::string& channel,
void (*handler)(const ReceiveBuffer *rbuf,
const std::string& channel,
ContextClass context),
ContextClass context) {
if(!this->lcm) {
fprintf(stderr, "LCM instance not initialized. Ignoring call to subscribeFunction()\n");
return NULL;
}
typedef LCMUntypedSubscription<ContextClass> SubsClass;
SubsClass *sub = new SubsClass();
sub->c_subs = lcm_subscribe(lcm, channel.c_str(), SubsClass::cb_func, sub);
sub->handler = handler;
sub->context = context;
subscriptions.push_back(sub);
return sub;
}
Subscription*
LCM::subscribe(const std::string& channel,
void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer* rbuf, const std::string& channel),
MessageHandlerClass* handler)
{
if(!this->lcm) {
fprintf(stderr,
"LCM instance not initialized. Ignoring call to subscribe()\n");
return NULL;
}
LCMMHUntypedSubscription<MessageHandlerClass> *subs =
new LCMMHUntypedSubscription<MessageHandlerClass>();
subs->handler = handler;
subs->handlerMethod = handlerMethod;
subs->c_subs = lcm_subscribe(this->lcm, channel.c_str(),
LCMMHUntypedSubscription<MessageHandlerClass>::cb_func, subs);
subscriptions.push_back(subs);
return subs;
}
static PyObject *
pylcm_subscribe (PyLCMObject *lcm_obj, PyObject *args)
{
char *channel = NULL;
int chan_len = 0;
PyObject *handler = NULL;
if (!PyArg_ParseTuple (args, "s#O", &channel, &chan_len, &handler))
return NULL;
if (!channel || ! chan_len) {
PyErr_SetString (PyExc_ValueError, "invalid channel");
return NULL;
}
if (!PyCallable_Check (handler)) {
PyErr_SetString (PyExc_ValueError, "handler is not callable");
return NULL;
}
PyLCMSubscriptionObject * subs_obj =
(PyLCMSubscriptionObject*) PyType_GenericNew (&pylcm_subscription_type,
NULL, NULL);
lcm_subscription_t *subscription =
lcm_subscribe (lcm_obj->lcm, channel, pylcm_msg_handler, subs_obj);
if (!subscription) {
Py_DECREF (subs_obj);
Py_RETURN_NONE;
}
subs_obj->subscription = subscription;
subs_obj->handler = handler;
Py_INCREF (handler);
subs_obj->lcm_obj = lcm_obj;
PyList_Append (lcm_obj->all_handlers, (PyObject*)subs_obj);
return (PyObject*)subs_obj;
}
int main(int argc, char **argv)
{
logplayer_t l;
double speed = 1.0;
int c;
char *expression = NULL;
struct option long_opts[] = {
{"help", no_argument, 0, 'h'}, {"speed", required_argument, 0, 's'},
{"lcm-url", required_argument, 0, 'l'}, {"verbose", no_argument, 0, 'v'},
{"regexp", required_argument, 0, 'e'}, {0, 0, 0, 0},
};
char *lcmurl = NULL;
memset(&l, 0, sizeof(logplayer_t));
while ((c = getopt_long(argc, argv, "hp:s:ve:l:", long_opts, 0)) >= 0) {
switch (c) {
case 's':
speed = strtod(optarg, NULL);
break;
case 'l':
free(lcmurl);
lcmurl = strdup(optarg);
break;
case 'v':
l.verbose = 1;
break;
case 'e':
expression = strdup(optarg);
break;
case 'h':
default:
usage(argv[0]);
return 1;
};
}
if (optind != argc - 1) {
usage(argv[0]);
return 1;
}
char *file = argv[optind];
printf("Using playback speed %f\n", speed);
if (!expression)
expression = strdup(".*");
#ifndef WIN32
char url_in[strlen(file) + 64];
#else
char url_in[2048];
#endif
sprintf(url_in, "file://%s?speed=%f", argv[optind], speed);
l.lcm_in = lcm_create(url_in);
if (!l.lcm_in) {
fprintf(stderr, "Error: Failed to open %s\n", file);
free(expression);
return 1;
}
l.lcm_out = lcm_create(lcmurl);
free(lcmurl);
if (!l.lcm_out) {
fprintf(stderr, "Error: Failed to create LCM\n");
free(expression);
return 1;
}
lcm_subscribe(l.lcm_in, expression, handler, &l);
while (!lcm_handle(l.lcm_in)) {
// Do nothing
}
lcm_destroy(l.lcm_in);
lcm_destroy(l.lcm_out);
free(expression);
}
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;
}
int LcmTunnel::connectToServer(lcm_t * lcm_, introspect_t *introspect_, GMainLoop * mainloop_, char * server_addr_str,
int port, char * channels_to_recv, lcm_tunnel_params_t * tunnel_params_, tunnel_server_params_t * server_params_)
{ //for a client that should initiate a connection with a server
tunnel_params = lcm_tunnel_params_t_copy(tunnel_params_);
server_params = server_params_;
lcm = lcm_;
introspect = introspect_;
mainloop = mainloop_;
if (tunnel_params->udp) {
// allocate UDP socket
udp_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (udp_fd < 0) {
perror("allocating UDP socket");
return 0;
}
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;
if (bind(udp_fd, (struct sockaddr*) &udp_addr, sizeof(udp_addr)) < 0) {
perror("binding UDP socket");
return 0;
}
getsockname(udp_fd, (struct sockaddr*) &udp_addr, &udp_addr_len);
tunnel_params->udp_port = ntohs(udp_addr.sin_port);
udp_ioc = g_io_channel_unix_new(udp_fd);
udp_sid = g_io_add_watch(udp_ioc, G_IO_IN, LcmTunnel::on_udp_data, this);
}
else {
udp_fd = -1;
}
// connect
tcp_sock = ssocket_create();
if (0 != ssocket_connect(tcp_sock, server_addr_str, port)) {
perror("connecting");
return 0;
}
tcp_ioc = g_io_channel_unix_new(ssocket_get_fd(tcp_sock));
tcp_sid = g_io_add_watch(tcp_ioc, G_IO_IN, on_tcp_data, this);
//fill out the name info
struct sockaddr_in server_addr;
socklen_t addrlen = sizeof(server_addr);
getpeername(tcp_sock->socket, (struct sockaddr*) &server_addr, &addrlen);
uint32_t server_port = ntohs(server_addr.sin_port);
snprintf(name, sizeof(name), "%s:%d", inet_ntoa(server_addr.sin_addr), server_port);
fprintf(stderr, "Connected to %s\n", name);
// transmit subscription information
lcm_tunnel_params_t * tun_params_to_send = lcm_tunnel_params_t_copy(tunnel_params);
//put the channels the server should send in the params we're sending it.
free(tun_params_to_send->channels);
tun_params_to_send->channels = strdup(channels_to_recv);
int msg_sz = lcm_tunnel_params_t_encoded_size(tun_params_to_send);
uint8_t * msg = (uint8_t *) calloc(msg_sz, sizeof(uint8_t));
lcm_tunnel_params_t_encode(msg, 0, msg_sz, tun_params_to_send);
uint32_t msg_sz_n = htonl(msg_sz);
if (4 != _fileutils_write_fully(ssocket_get_fd(tcp_sock), &msg_sz_n, 4)) {
perror("sending subscription data");
ssocket_destroy(tcp_sock);
free(msg);
lcm_tunnel_params_t_destroy(tun_params_to_send);
return 0;
}
if (msg_sz != _fileutils_write_fully(ssocket_get_fd(tcp_sock), msg, msg_sz)) {
perror("sending subscription data");
ssocket_destroy(tcp_sock);
free(msg);
lcm_tunnel_params_t_destroy(tun_params_to_send);
return 0;
}
lcm_tunnel_params_t_destroy(tun_params_to_send);
free(msg);
//set state for tcp receptions
bytes_to_read = 4;
bytes_read = 0;
if (tunnel_params->udp) {
tunnel_state = SERVER_MSG_SZ; //wait for udp port from server
}
else {
tunnel_state = RECV_CHAN_SZ;
//subscribe to the channels we want to send out
//only subscribe if we're doing TCP, since UDP socket hasn't been setup yet
subscription = lcm_subscribe(lcm, tunnel_params->channels, on_lcm_message, this);
}
return 1;
}
int main(int argc, char *argv[])
{
#ifndef WIN32
setlinebuf (stdout);
#endif
char logpath[PATH_MAX];
memset (logpath, 0, sizeof (logpath));
logger_t logger;
memset (&logger, 0, sizeof (logger));
// set some defaults
logger.force_overwrite = 0;
logger.auto_increment = 0;
logger.use_strftime = 0;
char *chan_regex = strdup(".*");
double max_write_queue_size_mb = DEFAULT_MAX_WRITE_QUEUE_SIZE_MB;
logger.invert_channels = 0;
logger.fflush_interval_ms = 100;
char *lcmurl = NULL;
char *optstring = "a:fic:shm:vu:";
int c;
struct option long_opts[] = {
{ "auto-split-hours", required_argument, 0, 'a' },
{ "auto-split-mb", required_argument, 0, 'b' },
{ "channel", required_argument, 0, 'c' },
{ "force", no_argument, 0, 'f' },
{ "increment", required_argument, 0, 'i' },
{ "lcm-url", required_argument, 0, 'l' },
{ "max-unwritten-mb", required_argument, 0, 'm' },
{ "strftime", required_argument, 0, 's' },
{ "invert-channels", no_argument, 0, 'v' },
{ "flush-interval", required_argument, 0,'u'},
{ 0, 0, 0, 0 }
};
while ((c = getopt_long (argc, argv, optstring, long_opts, 0)) >= 0)
{
switch (c) {
case 'a':
logger.auto_split_hours = strtod(optarg, NULL);
logger.auto_increment = 1;
if(logger.auto_split_hours <= 0) {
usage();
return 1;
}
break;
case 'b':
logger.auto_split_mb = strtod(optarg, NULL);
logger.auto_increment = 1;
if(logger.auto_split_mb <= 0) {
usage();
return 1;
}
break;
case 'f':
logger.force_overwrite = 1;
break;
case 'c':
free(chan_regex);
chan_regex = strdup(optarg);
break;
case 'i':
logger.auto_increment = 1;
break;
case 's':
logger.use_strftime = 1;
break;
case 'l':
free(lcmurl);
lcmurl = strdup(optarg);
break;
case 'v':
logger.invert_channels = 1;
break;
case 'm':
max_write_queue_size_mb = strtod(optarg, NULL);
if(max_write_queue_size_mb <= 0) {
usage();
return 1;
}
break;
case 'u':
logger.fflush_interval_ms = atol(optarg);
if(logger.fflush_interval_ms <= 0) {
usage();
return 1;
}
break;
case 'h':
default:
usage();
return 1;
};
}
if (optind == argc) {
strcpy (logger.input_fname, "lcmlog-%Y-%m-%d");
logger.auto_increment = 1;
logger.use_strftime = 1;
} else if (optind == argc - 1) {
strncpy (logger.input_fname, argv[optind], sizeof (logger.input_fname));
} else if (optind < argc-1) {
usage ();
return 1;
}
// initialize GLib threading
g_thread_init(NULL);
logger.time0 = timestamp_now();
logger.max_write_queue_size = (int64_t)(max_write_queue_size_mb * (1 << 20));
if(0 != open_logfile(&logger))
return 1;
// create write thread
logger.write_thread_exit_flag = 0;
logger.mutex = g_mutex_new();
logger.write_queue_size = 0;
logger.write_queue = g_async_queue_new();
logger.write_thread = g_thread_create(write_thread, &logger, TRUE, NULL);
// begin logging
logger.lcm = lcm_create (lcmurl);
free(lcmurl);
if (!logger.lcm) {
fprintf (stderr, "Couldn't initialize LCM!");
return 1;
}
if(logger.invert_channels) {
// if inverting the channels, subscribe to everything and invert on the
// callback
lcm_subscribe(logger.lcm, ".*", message_handler, &logger);
char *regexbuf = g_strdup_printf("^%s$", chan_regex);
#ifdef USE_GREGEX
GError *rerr = NULL;
logger.regex = g_regex_new(regexbuf, (GRegexCompileFlags) 0, (GRegexMatchFlags) 0, &rerr);
if(rerr) {
fprintf(stderr, "%s\n", rerr->message);
g_free(regexbuf);
return 1;
}
#else
if (0 != regcomp (&logger.preg, regexbuf, REG_NOSUB | REG_EXTENDED)) {
fprintf(stderr, "bad regex!\n");
g_free(regexbuf);
return 1;
}
#endif
g_free(regexbuf);
} else {
// otherwise, let LCM handle the regex
lcm_subscribe(logger.lcm, chan_regex, message_handler, &logger);
}
free(chan_regex);
_mainloop = g_main_loop_new (NULL, FALSE);
signal_pipe_glib_quit_on_kill ();
glib_mainloop_attach_lcm (logger.lcm);
#ifdef USE_SIGHUP
signal(SIGHUP, sighup_handler);
#endif
// main loop
g_main_loop_run (_mainloop);
fprintf(stderr, "Logger exiting\n");
// stop the write thread
g_mutex_lock(logger.mutex);
logger.write_thread_exit_flag = 1;
g_mutex_unlock(logger.mutex);
g_async_queue_push(logger.write_queue, &logger.write_thread_exit_flag);
g_thread_join(logger.write_thread);
g_mutex_free(logger.mutex);
// cleanup. This isn't strictly necessary, do it to be pedantic and so that
// leak checkers don't complain
glib_mainloop_detach_lcm (logger.lcm);
lcm_destroy (logger.lcm);
lcm_eventlog_destroy (logger.log);
for(void *msg = g_async_queue_try_pop(logger.write_queue); msg;
msg=g_async_queue_try_pop(logger.write_queue)) {
if(msg == &logger.write_thread_exit_flag)
continue;
free(msg);
}
g_async_queue_unref(logger.write_queue);
if(logger.invert_channels) {
#ifdef USE_GREGEX
g_regex_unref(logger.regex);
#else
regfree(&logger.preg);
#endif
}
return 0;
}
