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