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 *
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 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;
}
