static int lcm_memq_handle(lcm_memq_t *self)
{
char ch;
int status = lcm_internal_pipe_read(self->notify_pipe[0], &ch, 1);
if (status == 0) {
fprintf(stderr, "Error: lcm_memq_handle read 0 bytes from notify_pipe\n");
return -1;
}
g_mutex_lock(self->mutex);
memq_msg_t *msg = (memq_msg_t *) g_queue_pop_head(self->queue);
if (!g_queue_is_empty(self->queue)) {
if (lcm_internal_pipe_write(self->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write to notify pipe (lcm_memq_handle)");
}
}
g_mutex_unlock(self->mutex);
dbg(DBG_LCM, "Dispatching message on channel [%s], size [%d]\n", msg->channel,
msg->rbuf.data_size);
if (lcm_try_enqueue_message(self->lcm, msg->channel)) {
lcm_dispatch_handlers(self->lcm, &msg->rbuf, msg->channel);
}
memq_msg_destroy(msg);
return 0;
}
static int
lcm_udpm_handle (lcm_udpm_t *lcm)
{
int status;
char ch;
if(0 != _setup_recv_parts (lcm))
return -1;
/* Read one byte from the notify pipe. This will block if no packets are
* available yet and wake up when they are. */
status = lcm_internal_pipe_read(lcm->notify_pipe[0], &ch, 1);
if (status == 0) {
fprintf (stderr, "Error: lcm_handle read 0 bytes from notify_pipe\n");
return -1;
}
else if (status < 0) {
fprintf (stderr, "Error: lcm_handle read: %s\n", strerror (errno));
return -1;
}
/* Dequeue the next received packet */
g_static_rec_mutex_lock (&lcm->mutex);
lcm_buf_t * lcmb = lcm_buf_dequeue (lcm->inbufs_filled);
if (!lcmb) {
fprintf (stderr,
"Error: no packet available despite getting notification.\n");
g_static_rec_mutex_unlock (&lcm->mutex);
return -1;
}
/* If there are still packets in the queue, put something back in the pipe
* so that future invocations will get called. */
if (!lcm_buf_queue_is_empty (lcm->inbufs_filled))
if (lcm_internal_pipe_write(lcm->notify_pipe[1], "+", 1) < 0)
perror ("write to notify");
g_static_rec_mutex_unlock (&lcm->mutex);
lcm_recv_buf_t rbuf;
rbuf.data = (uint8_t*) lcmb->buf + lcmb->data_offset;
rbuf.data_size = lcmb->data_size;
rbuf.recv_utime = lcmb->recv_utime;
rbuf.lcm = lcm->lcm;
if(lcm->creating_read_thread) {
// special case: If we're creating the read thread and are in
// self-test mode, then only dispatch the self-test message.
if(!strcmp(lcmb->channel_name, SELF_TEST_CHANNEL))
lcm_dispatch_handlers (lcm->lcm, &rbuf, lcmb->channel_name);
} else {
lcm_dispatch_handlers (lcm->lcm, &rbuf, lcmb->channel_name);
}
g_static_rec_mutex_lock (&lcm->mutex);
lcm_buf_free_data(lcmb, lcm->ringbuf);
lcm_buf_enqueue (lcm->inbufs_empty, lcmb);
g_static_rec_mutex_unlock (&lcm->mutex);
return 0;
}
static void
lcm_logprov_destroy (lcm_logprov_t *lr)
{
dbg (DBG_LCM, "closing lcm log provider context\n");
if (lr->thread_created) {
/* Destroy the timer thread */
int64_t abort_cmd = -1;
int status = lcm_internal_pipe_write(lr->timer_pipe[1], &abort_cmd, sizeof(abort_cmd));
if(status < 0) {
perror(__FILE__ " - write (abort_cmd)");
}
g_thread_join (lr->timer_thread);
}
if(lr->notify_pipe[0] >= 0) lcm_internal_pipe_close(lr->notify_pipe[0]);
if(lr->notify_pipe[1] >= 0) lcm_internal_pipe_close(lr->notify_pipe[1]);
if(lr->timer_pipe[0] >= 0) lcm_internal_pipe_close(lr->timer_pipe[0]);
if(lr->timer_pipe[1] >= 0) lcm_internal_pipe_close(lr->timer_pipe[1]);
if (lr->event)
lcm_eventlog_free_event (lr->event);
if (lr->log)
lcm_eventlog_destroy (lr->log);
free (lr->filename);
free (lr);
}
/* This is the receiver thread that runs continuously to retrieve any incoming
* LCM packets from the network and queues them locally. */
static void *
recv_thread (void * user)
{
#ifdef G_OS_UNIX
// Mask out all signals on this thread.
sigset_t mask;
sigfillset(&mask);
pthread_sigmask(SIG_SETMASK, &mask, NULL);
#endif
lcm_udpm_t * lcm = (lcm_udpm_t *) user;
while (1) {
lcm_buf_t *lcmb = udp_read_packet(lcm);
if (!lcmb) break;
/* If necessary, notify the reading thread by writing to a pipe. We
* only want one character in the pipe at a time to avoid blocking
* writes, so we only do this when the queue transitions from empty to
* non-empty. */
g_static_rec_mutex_lock (&lcm->mutex);
if (lcm_buf_queue_is_empty (lcm->inbufs_filled))
if (lcm_internal_pipe_write(lcm->notify_pipe[1], "+", 1) < 0)
perror ("write to notify");
/* Queue the packet for future retrieval by lcm_handle (). */
lcm_buf_enqueue (lcm->inbufs_filled, lcmb);
g_static_rec_mutex_unlock (&lcm->mutex);
}
dbg (DBG_LCM, "read thread exiting\n");
return NULL;
}
static void *
timer_thread (void * user)
{
lcm_logprov_t * lr = (lcm_logprov_t *) user;
int64_t abstime;
struct timeval sleep_tv;
while (lcm_internal_pipe_read(lr->timer_pipe[0], &abstime, 8) == 8) {
if (abstime < 0) return NULL;
int64_t now = timestamp_now();
if (abstime > now) {
int64_t sleep_utime = abstime - now;
sleep_tv.tv_sec = sleep_utime / 1000000;
sleep_tv.tv_usec = sleep_utime % 1000000;
// sleep until the next timed message, or until an abort message
fd_set fds;
FD_ZERO (&fds);
FD_SET (lr->timer_pipe[0], &fds);
int status = select (lr->timer_pipe[0] + 1, &fds, NULL, NULL,
&sleep_tv);
if (0 == status) {
// select timed out
if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write (timer select)");
}
}
} else {
if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write (timer)");
}
}
}
perror ("timer_thread read failed");
return NULL;
}
static void
_destroy_recv_parts (lcm_udpm_t *lcm)
{
if (lcm->thread_created) {
// send the read thread an exit command
int wstatus = lcm_internal_pipe_write(lcm->thread_msg_pipe[1], "\0", 1);
if(wstatus < 0) {
perror(__FILE__ " write(destroy)");
} else {
g_thread_join (lcm->read_thread);
}
lcm->read_thread = NULL;
lcm->thread_created = 0;
}
if (lcm->thread_msg_pipe[0] >= 0) {
lcm_internal_pipe_close(lcm->thread_msg_pipe[0]);
lcm_internal_pipe_close(lcm->thread_msg_pipe[1]);
lcm->thread_msg_pipe[0] = lcm->thread_msg_pipe[1] = -1;
}
if (lcm->recvfd >= 0) {
lcm_close_socket(lcm->recvfd);
lcm->recvfd = -1;
}
if (lcm->frag_bufs) {
lcm_frag_buf_store_destroy(lcm->frag_bufs);
lcm->frag_bufs = NULL;
}
if (lcm->inbufs_empty) {
lcm_buf_queue_free (lcm->inbufs_empty, lcm->ringbuf);
lcm->inbufs_empty = NULL;
}
if (lcm->inbufs_filled) {
lcm_buf_queue_free (lcm->inbufs_filled, lcm->ringbuf);
lcm->inbufs_filled = NULL;
}
if (lcm->ringbuf) {
lcm_ringbuf_free (lcm->ringbuf);
lcm->ringbuf = NULL;
}
}
static int lcm_memq_publish(lcm_memq_t *self, const char *channel, const void *data,
unsigned int datalen)
{
if (!lcm_has_handlers(self->lcm, channel)) {
dbg(DBG_LCM, "Publishing [%s] size [%d] - dropping (no subscribers)\n", channel, datalen);
return 0;
}
dbg(DBG_LCM, "Publishing to [%s] message size [%d]\n", channel, datalen);
memq_msg_t *msg = memq_msg_new(self->lcm, channel, data, datalen, timestamp_now());
g_mutex_lock(self->mutex);
int was_empty = g_queue_is_empty(self->queue);
g_queue_push_tail(self->queue, msg);
if (was_empty) {
if (lcm_internal_pipe_write(self->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write to notify pipe (lcm_memq_publish)");
}
}
g_mutex_unlock(self->mutex);
return 0;
}
static int
lcm_udpm_handle (lcm_udpm_t *lcm)
{
int status;
char ch;
if (! lcm->thread_created) {
if (0 != _setup_recv_thread (lcm))
return -1;
}
/* Read one byte from the notify pipe. This will block if no packets are
* available yet and wake up when they are. */
status = lcm_internal_pipe_read(lcm->notify_pipe[0], &ch, 1);
if (status == 0) {
fprintf (stderr, "Error: lcm_handle read 0 bytes from notify_pipe\n");
return -1;
}
else if (status < 0) {
fprintf (stderr, "Error: lcm_handle read: %s\n", strerror (errno));
return -1;
}
/* Dequeue the next received packet */
g_static_rec_mutex_lock (&lcm->mutex);
lcm_buf_t * lcmb = lcm_buf_dequeue (lcm->inbufs_filled);
if (!lcmb) {
fprintf (stderr,
"Error: no packet available despite getting notification.\n");
g_static_rec_mutex_unlock (&lcm->mutex);
return -1;
}
/* If there are still packets in the queue, put something back in the pipe
* so that future invocations will get called. */
if (!is_buf_queue_empty (lcm->inbufs_filled))
if (lcm_internal_pipe_write(lcm->notify_pipe[1], "+", 1) < 0)
perror ("write to notify");
g_static_rec_mutex_unlock (&lcm->mutex);
lcm_recv_buf_t rbuf;
rbuf.data = (uint8_t*) lcmb->buf + lcmb->data_offset;
rbuf.data_size = lcmb->data_size;
rbuf.recv_utime = lcmb->recv_utime;
rbuf.lcm = lcm->lcm;
lcm_dispatch_handlers (lcm->lcm, &rbuf, lcmb->channel_name);
g_static_rec_mutex_lock (&lcm->mutex);
if (lcmb->buf_from_ringbuf)
lcm_ringbuf_dealloc (lcm->ringbuf, lcmb->buf);
else
free (lcmb->buf);
lcmb->buf = NULL;
lcmb->buf_size = 0;
lcm_buf_enqueue (lcm->inbufs_empty, lcmb);
g_static_rec_mutex_unlock (&lcm->mutex);
return 0;
}
static int
lcm_logprov_handle (lcm_logprov_t * lr)
{
lcm_recv_buf_t rbuf;
if (!lr->event)
return -1;
char ch;
int status = lcm_internal_pipe_read(lr->notify_pipe[0], &ch, 1);
if (status == 0) {
fprintf (stderr, "Error: lcm_handle read 0 bytes from notify_pipe\n");
return -1;
}
else if (status < 0) {
fprintf (stderr, "Error: lcm_handle read: %s\n", strerror (errno));
return -1;
}
int64_t now = timestamp_now ();
/* Initialize the wall clock if this is the first time through */
if (lr->next_clock_time < 0)
lr->next_clock_time = now;
// rbuf.channel = lr->event->channel,
rbuf.data = (uint8_t*) lr->event->data;
rbuf.data_size = lr->event->datalen;
rbuf.recv_utime = lr->next_clock_time;
rbuf.lcm = lr->lcm;
if(lcm_try_enqueue_message(lr->lcm, lr->event->channel))
lcm_dispatch_handlers (lr->lcm, &rbuf, lr->event->channel);
int64_t prev_log_time = lr->event->timestamp;
if (load_next_event (lr) < 0) {
/* end-of-file reached. This call succeeds, but next call to
* _handle will fail */
lr->event = NULL;
if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write(notify)");
}
return 0;
}
/* Compute the wall time for the next event */
if (lr->speed > 0)
lr->next_clock_time +=
(lr->event->timestamp - prev_log_time) / lr->speed;
else
lr->next_clock_time = now;
if (lr->next_clock_time > now) {
int wstatus = lcm_internal_pipe_write(lr->timer_pipe[1], &lr->next_clock_time, 8);
if(wstatus < 0) {
perror(__FILE__ " - write(timer_pipe)");
}
} else {
int wstatus = lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1);
if(wstatus < 0) {
perror(__FILE__ " - write(notify_pipe)");
}
}
return 0;
}
static lcm_provider_t *
lcm_logprov_create (lcm_t * parent, const char *target, const GHashTable *args)
{
if (!target || !strlen (target)) {
fprintf (stderr, "Error: Missing filename\n");
return NULL;
}
lcm_logprov_t * lr = (lcm_logprov_t *) calloc (1, sizeof (lcm_logprov_t));
lr->lcm = parent;
lr->filename = strdup(target);
lr->speed = 1;
lr->next_clock_time = -1;
lr->start_timestamp = -1;
g_hash_table_foreach ((GHashTable*) args, new_argument, lr);
dbg (DBG_LCM, "Initializing LCM log provider context...\n");
dbg (DBG_LCM, "Filename %s\n", lr->filename);
if(lcm_internal_pipe_create(lr->notify_pipe) != 0) {
perror(__FILE__ " - pipe (notify)");
lcm_logprov_destroy (lr);
return NULL;
}
if(lcm_internal_pipe_create(lr->timer_pipe) != 0) {
perror(__FILE__ " - pipe (timer)");
lcm_logprov_destroy (lr);
return NULL;
}
//fcntl (lcm->notify_pipe[1], F_SETFL, O_NONBLOCK);
if (!lr->writer) {
lr->log = lcm_eventlog_create (lr->filename, "r");
} else {
lr->log = lcm_eventlog_create (lr->filename, "w");
}
if (!lr->log) {
fprintf (stderr, "Error: Failed to open %s: %s\n", lr->filename,
strerror (errno));
lcm_logprov_destroy (lr);
return NULL;
}
// only start the reader thread if not in write mode
if (!lr->writer){
if (load_next_event (lr) < 0) {
fprintf (stderr, "Error: Failed to read first event from log\n");
lcm_logprov_destroy (lr);
return NULL;
}
/* Start the reader thread */
lr->timer_thread = g_thread_create (timer_thread, lr, TRUE, NULL);
if (!lr->timer_thread) {
fprintf (stderr, "Error: LCM failed to start timer thread\n");
lcm_logprov_destroy (lr);
return NULL;
}
lr->thread_created = 1;
if(lcm_internal_pipe_write(lr->notify_pipe[1], "+", 1) < 0) {
perror(__FILE__ " - write (reader create)");
}
if(lr->start_timestamp > 0){
dbg (DBG_LCM, "Seeking to timestamp: %lld\n", (long long)lr->start_timestamp);
lcm_eventlog_seek_to_timestamp(lr->log, lr->start_timestamp);
}
}
return lr;
}
