void LcmTunnel::checkUDPSendStatus(int send_status)
{
int64_t now = _timestamp_now();
if (send_status < 0) {
if (errorStartTime < 0)
errorStartTime = now;
if (now - lastErrorPrintTime > 1e6) {
fprintf(stderr, "errno %d, send_status %d :", errno, send_status);
perror("error sending UDP Data: ");
errno = 0;
lastErrorPrintTime = now;
}
numSuccessful = 0;
}
else if (lastErrorPrintTime > 0) {
if (numSuccessful > 3) {
fprintf(stderr, "Connection may be back up after %fsec send_status=%d numSuccessful=%d\n", (double) (now
- errorStartTime) * 1e-6, send_status, numSuccessful);
perror("perror: ");
lastErrorPrintTime = -1;
errorStartTime = -1;
errno = 0;
}
numSuccessful++;
}
}
static void
on_frame_ready (CamUnitChain *chain, CamUnit *unit,
const CamFrameBuffer *buf, void *user_data)
{
state_t *s = (state_t*) user_data;
const CamUnitFormat *fmt = cam_unit_get_output_format (unit);
int64_t now = _timestamp_now ();
if (now > s->next_publish_utime) {
camlcm_image_t msg;
memset (&msg, 0, sizeof (msg));
msg.width = fmt->width;
msg.height = fmt->height;
msg.row_stride = fmt->row_stride;
msg.data = buf->data;
msg.size = buf->bytesused;
msg.pixelformat = fmt->pixelformat;
camlcm_image_t_publish (s->lcm, "PHONE_THUMB", &msg);
s->next_publish_utime = now + (int64_t) PUBLISH_INTERVAL_USEC;
printf ("publish\n");
}
}
gpointer LcmTunnel::sendThreadFunc(gpointer user_data)
{
LcmTunnel *self = (LcmTunnel*) user_data;
g_mutex_lock(self->sendQueueLock);
int64_t nextFlushTime = 0;
while (!self->stopSendThread) {
if (self->sendQueue.empty()) {
g_cond_wait(self->sendQueueCond, self->sendQueueLock);
nextFlushTime = _timestamp_now() + self->tunnel_params->max_delay_ms * 1000;
continue;
}
int64_t now = _timestamp_now();
if (self->tunnel_params->max_delay_ms > 0 && self->bytesInQueue < NUM_BYTES_TO_SEND_IMMEDIATELY && nextFlushTime
> now && !self->flushImmediately) {
GTimeVal next_timeout;
_timestamp_to_GTimeVal(nextFlushTime, &next_timeout);
g_cond_timed_wait(self->sendQueueCond, self->sendQueueLock, &next_timeout);
continue;
}
//there is stuff in the queue that we need to handle
self->flushImmediately = false;
//take current contents out of the queue
std::deque<TunnelLcmMessage *> tmpQueue;
tmpQueue.swap(self->sendQueue);
uint32_t bytesInTmpQueue = self->bytesInQueue;
self->bytesInQueue = 0;
g_mutex_unlock(self->sendQueueLock);
//release lock for sending
//process whats in the queue
bool success = self->send_lcm_messages(tmpQueue, bytesInTmpQueue);
//reaquire lock to go around the loop
g_mutex_lock(self->sendQueueLock);
if (!success)
break;
}
g_mutex_unlock(self->sendQueueLock);
g_thread_exit(NULL);
}
static gboolean
cam_input_example_try_produce_frame (CamUnit *super)
{
dbg(DBG_INPUT, "iterate\n");
CamInputExample *self = (CamInputExample*)super;
int64_t now = _timestamp_now ();
if (now < self->next_frame_time) return FALSE;
int64_t frame_delay_usec = 1000000. / self->fps;
self->next_frame_time += frame_delay_usec;
const CamUnitFormat *outfmt = cam_unit_get_output_format(super);
int buf_sz = outfmt->height * outfmt->row_stride;
CamFrameBuffer *outbuf = cam_framebuffer_new_alloc (buf_sz);
memset (outbuf->data, 0, buf_sz);
self->x += self->dx;
int w = cam_unit_control_get_int (self->int1_ctl);
int excess = self->x + w - outfmt->width + 1;
if (excess > 0) {
self->x = outfmt->width - 1 - excess - w;
self->dx *= -1;
} else if (self->x < 0) {
self->x = - self->x;
self->dx *= -1;
}
self->y += self->dy;
excess = self->y + w - outfmt->height + 1;
if (excess > 0) {
self->y = outfmt->height - 1 - excess - w;
self->dy *= -1;
} else if (self->y < 0) {
self->y = - self->y;
self->dy *= -1;
}
uint8_t rgb[3];
if (cam_unit_control_get_boolean (self->bool_ctl)) {
rgb[0] = cam_unit_control_get_int (self->int2_ctl);
rgb[1] = 0;
rgb[2] = 0;
} else {
rgb[0] = 0;
rgb[1] = rgb[2] = cam_unit_control_get_int (self->int2_ctl);
};
_draw_rectangle (outbuf, outfmt, self->x, self->y, w, w, rgb);
outbuf->bytesused = buf_sz;
outbuf->timestamp = now;
cam_unit_produce_frame (super, outbuf, outfmt);
g_object_unref (outbuf);
return TRUE;
}
static int
cam_input_example_stream_init (CamUnit *super, const CamUnitFormat *fmt)
{
dbg(DBG_INPUT, "example stream init\n");
CamInputExample *self = (CamInputExample*)super;
self->next_frame_time = _timestamp_now();
return 0;
}
void LcmTunnel::send_to_remote(const void *data, uint32_t len, const char *lcm_channel)
{
lcm_recv_buf_t rbuf;
rbuf.data = (void *) data;
rbuf.data_size = len;
rbuf.recv_utime = _timestamp_now(); //use current timestamp, should be close to when received...
rbuf.lcm = this->lcm;
send_to_remote(&rbuf, lcm_channel);
}
void ModelClient::doModelClient(){
model_pub_robot_urdf_t_subscription_t * sub = model_pub_robot_urdf_t_subscribe(lcm_, model_channel_.c_str(), robot_urdf_handler_aux, this);
//TODO: is there a way to be sure nothing else is subscribed???
int64_t utime_start = _timestamp_now();
int64_t last_print_utime = -1;
while ((_timestamp_now() - utime_start) < 2.5e6) {
//bot_param_request_t req;
//req.utime = _timestamp_now();
//bot_param_request_t_publish(lcm, request_channel, &req);
lcm_sleep(lcm_, .25);
if (urdf_parsed_)
break;
int64_t now = _timestamp_now();
if (now - utime_start > 5e5) {
if (last_print_utime < 0) {
fprintf(stderr, "ModelClient waiting to get model from robot_model_publisher...");
last_print_utime = now;
}
else if (now - last_print_utime > 5e5) {
fprintf(stderr, ".");
last_print_utime = now;
}
}
}
if (last_print_utime > 0) {
fprintf(stderr, "\n");
}
if (!urdf_parsed_) {
fprintf(stderr,
"WARNING: ModelClient could not get model from the robot_model_publisher!\n Did you forget to start one?\n");
exit(-1);
return;
}
if (!keep_updated_) {
//unsubscribe from urdf messages
model_pub_robot_urdf_t_unsubscribe(lcm_, sub);
// param->server_id = -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;
}
static gboolean
cam_example_try_set_control(CamUnit *super,
const CamUnitControl *ctl, const GValue *proposed, GValue *actual)
{
CamInputExample *self = (CamInputExample*)super;
if (ctl == self->enum_ctl) {
self->fps = fps_numer_options[ g_value_get_int(proposed) ];
self->next_frame_time = _timestamp_now();
}
g_value_copy (proposed, actual);
return TRUE;
}
static inline void
bot_fasttrig_atan2_test()
{
double max_err = 0;
int64_t start = _timestamp_now();
for (int i = 0; i < 10000000; i++) {
double x = signed_randf();
double y = signed_randf();
double a = atan2(y,x);
double b = bot_fasttrig_atan2(y,x);
double err = fabs(a-b);
if (err > max_err) {
printf("max err: %15f deg (%15f %15f %15f %15f)\n", to_degrees(err), x, y, a, b);
max_err = err;
}
}
int64_t stop = _timestamp_now();
printf("dt: %15f\n", (stop-start)/1000000.0);
}
void publish_params(param_server_t *self)
{
bot_param_update_t * update_msg = (bot_param_update_t *) calloc(1, sizeof(bot_param_update_t));
int ret = bot_param_write_to_string(self->params, &update_msg->params);
if (ret) {
fprintf(stderr, "ERROR: could not write message to string");
exit(1);
}
update_msg->utime = _timestamp_now();
update_msg->server_id = self->id;
update_msg->sequence_number = self->seqNo;
bot_param_update_t_publish(self->lcm, self->update_channel, update_msg);
bot_param_update_t_destroy(update_msg);
fprintf(stderr, ".");
}
bool LcmTunnel::send_lcm_messages(std::deque<TunnelLcmMessage *> &msgQueue, uint32_t bytesInQueue)
{
if (udp_fd >= 0) {
if (server_udp_port <= 0)
return true; //connection hasn't been setup yet.
udp_send_seqno++;//increment the sequence counter
udp_send_seqno = udp_send_seqno % SEQNO_WRAP_VAL;
if (verbose)
printf("sending %d bytes from %d lcm messages\n", bytesInQueue, static_cast<int> (msgQueue.size()));
uint32_t msgSize = bytesInQueue;
int nfragments = getNumFragments(msgSize);
if ((tunnel_params->fec <= 0 && nfragments > MAX_NUM_FRAGMENTS) || (tunnel_params->fec > 0 && nfragments
> MAX_NUM_FRAGMENTS / tunnel_params->fec)) {
uint32_t maxMsgSize;
if (tunnel_params->fec > 0)
maxMsgSize = MAX_PAYLOAD_BYTES_PER_FRAGMENT * MAX_NUM_FRAGMENTS / tunnel_params->fec;
else
maxMsgSize = MAX_PAYLOAD_BYTES_PER_FRAGMENT * MAX_NUM_FRAGMENTS;
fprintf(stderr,
"WARNING! Queue contains more than the max message size of %d bytes... we're WAY behind, dropping msgs\n",
maxMsgSize);
while (msgSize < maxMsgSize) {
//drop messages
TunnelLcmMessage * drop_msg = msgQueue.front();
msgQueue.pop_front();
msgSize -= drop_msg->encoded_size;
delete drop_msg;
}
}
//put the entire queue into 1 big buffer
uint8_t * msgBuf = (uint8_t *) malloc(msgSize * sizeof(uint8_t));
uint32_t msgBufOffset = 0;
while (!msgQueue.empty()) {
TunnelLcmMessage * msg = msgQueue.front();
msgQueue.pop_front();
lcm_tunnel_sub_msg_t_encode(msgBuf, msgBufOffset, msgSize - msgBufOffset, msg->sub_msg);
msgBufOffset += msg->encoded_size;
delete msg;
}
assert(msgBufOffset==msgSize);
if (tunnel_params->fec < 1 || nfragments < MIN_NUM_FRAGMENTS_FOR_FEC) { //don't use FEC
int sendRepeats = 1;
if (fabs(tunnel_params->fec) > 1) { //fec <0 means always send duplicates
sendRepeats = (int) ceil(fabs(tunnel_params->fec)); //send ceil of the fec rate times
}
for (int r = 0; r < sendRepeats; r++) {
msgBufOffset = 0;
for (int i = 0; i < nfragments; i++) {
lcm_tunnel_udp_msg_t msg;
msg.seqno = udp_send_seqno;
msg.fragno = i;
msg.payload_size = msgSize;
msg.data_size = MIN(MAX_PAYLOAD_BYTES_PER_FRAGMENT, msgSize - msgBufOffset);
msg.data = msgBuf + msgBufOffset;
msgBufOffset += msg.data_size;
int msg_sz = lcm_tunnel_udp_msg_t_encoded_size(&msg);
uint8_t msg_buf[msg_sz];
lcm_tunnel_udp_msg_t_encode(msg_buf, 0, msg_sz, &msg);
// printf("sending: %d, %d / %d\n", msg.seqno, msg.fragment, msg.nfrags);
int send_status = send(udp_fd, msg_buf, msg_sz, 0);
// fprintf(stderr,"sent packet\n");
checkUDPSendStatus(send_status);
}
}
}
else { //use tunnel error correction to send
ldpc_enc_wrapper * ldpc_enc = new ldpc_enc_wrapper(msgBuf, msgSize, MAX_PAYLOAD_BYTES_PER_FRAGMENT,
tunnel_params->fec);
lcm_tunnel_udp_msg_t msg;
msg.seqno = udp_send_seqno;
msg.payload_size = msgSize;
// printf("sending: %d, %d / %d\n", recv_udp_msg->seqno, recv_udp_msg->fragment, recv_udp_msg->nfrags);
int enc_done = 0;
while (!enc_done) {
uint8_t data_buf[MAX_PAYLOAD_BYTES_PER_FRAGMENT];
msg.data_size = MAX_PAYLOAD_BYTES_PER_FRAGMENT;
msg.data = data_buf;
enc_done = ldpc_enc->getNextPacket(msg.data, &msg.fragno);
int msg_sz = lcm_tunnel_udp_msg_t_encoded_size(&msg);
uint8_t msg_buf[msg_sz];
lcm_tunnel_udp_msg_t_encode(msg_buf, 0, msg_sz, &msg);
// printf("sending: %d, %d / %d\n", msg.seqno, msg.fragment, msg.nfrags);
int send_status = send(udp_fd, msg_buf, msg_sz, 0);
checkUDPSendStatus(send_status);
}
// printf("finished encoding and sending packet %d for channel: %s\n",recv_udp_msg->seqno,channel);
delete ldpc_enc;
}
free(msgBuf);
}
else {
int cfd = ssocket_get_fd(tcp_sock);
assert(cfd>0);
while (!msgQueue.empty()) {
TunnelLcmMessage * msg = msgQueue.front();
msgQueue.pop_front();
int64_t now = _timestamp_now();
double age_ms = (now - msg->recv_utime) * 1.0e-3;
if (tunnel_params->tcp_max_age_ms > 0 && age_ms > tunnel_params->tcp_max_age_ms) {
// message has been queued up for too long. Drop it.
if (verbose)
fprintf(stderr, "%s message too old (age = %d, param = %d), dropping.\n", msg->sub_msg->channel,
(int) age_ms, tunnel_params->tcp_max_age_ms);
}
else {
// send channel
int chan_len = strlen(msg->sub_msg->channel);
uint32_t chan_len_n = htonl(chan_len);
if (4 != _fileutils_write_fully(cfd, &chan_len_n, 4)) {
delete msg;
return false;
}
if (chan_len != _fileutils_write_fully(cfd, msg->sub_msg->channel, chan_len)) {
delete msg;
return false;
}
// send data
int data_size_n = htonl(msg->sub_msg->data_size);
if (4 != _fileutils_write_fully(cfd, &data_size_n, 4)) {
delete msg;
return false;
}
if (msg->sub_msg->data_size != _fileutils_write_fully(cfd, msg->sub_msg->data, msg->sub_msg->data_size)) {
delete msg;
return false;
}
}
if (verbose)
printf("Sent \"%s\".\n", msg->sub_msg->channel);
delete msg;
}
}
return true;
}
int main(int argc, char ** argv)
{
if (argc < 2) {
usage (argc, argv);
exit(1);
}
param_server_t * self = calloc(1, sizeof(param_server_t));
GMainLoop * mainloop = g_main_loop_new(NULL, FALSE);
if (!mainloop) {
fprintf (stderr, "Error getting the GLIB main loop\n");
exit(1);
}
char *optstring = "hs:l:m::";
struct option long_opts[] = {
{ "help", no_argument, NULL, 'h' },
{ "server-name", required_argument, NULL, 's' },
{ "lcm-url", required_argument, NULL, 'l' },
{ "message-init", optional_argument, NULL, 'm' },
{ 0, 0, 0, 0 }
};
int c=-1;
char *param_prefix = NULL;
char *lcm_url = NULL;
char *message_init_channel = NULL;
while ((c = getopt_long (argc, argv, optstring, long_opts, 0)) >= 0)
{
switch (c) {
case 's':
param_prefix = optarg;
break;
case 'l':
lcm_url = optarg;
break;
case 'm':
message_init_channel = (optarg==NULL) ? BOT_PARAM_UPDATE_CHANNEL : optarg;
break;
case 'h':
default:
usage (argc, argv);
return 1;
}
}
self->lcm = lcm_create(lcm_url);
lcmu_glib_mainloop_attach_lcm(self->lcm);
self->seqNo = 0;
self->id = _timestamp_now();
if (message_init_channel == NULL) {
self->params = bot_param_new_from_file(argv[optind]);
if (self->params==NULL){
fprintf(stderr, "Could not load params from %s\n", argv[optind]);
exit(1);
}
else{
fprintf(stderr, "Loaded params from %s\n", argv[optind]);
}
}
// set channels here
if (!param_prefix) param_prefix = getenv ("BOT_PARAM_SERVER_NAME");
self->update_channel = g_strconcat (param_prefix ? : "",
BOT_PARAM_UPDATE_CHANNEL, NULL);
self->request_channel = g_strconcat (param_prefix ? : "",
BOT_PARAM_REQUEST_CHANNEL, NULL);
self->set_channel = g_strconcat (param_prefix ? : "",
BOT_PARAM_SET_CHANNEL, NULL);
bot_param_update_t_subscribe(self->lcm, self->update_channel, on_param_update, (void *) self);
bot_param_request_t_subscribe(self->lcm, self->request_channel, on_param_request, (void *) self);
bot_param_set_t_subscribe(self->lcm, self->set_channel, on_param_set, (void *) self);
if (message_init_channel != NULL) {
fprintf(stderr, "Listening to %s for params\n", message_init_channel);
self->init_subscription = bot_param_update_t_subscribe(self->lcm, message_init_channel, on_param_init, (void *)self);
}
//timer to always publish params every 5sec
g_timeout_add_full(G_PRIORITY_HIGH, (guint) 5.0 * 1000, on_timer, (gpointer) self, NULL);
g_main_loop_run(mainloop);
return 0;
}
// read continuously until a complete message arrives
static lcm_buf_t *
udp_read_packet (lcm_udpm_t *lcm)
{
lcm_buf_t *lcmb = NULL;
int sz = 0;
g_static_rec_mutex_lock (&lcm->mutex);
double buf_avail = lcm_ringbuf_available(lcm->ringbuf);
g_static_rec_mutex_unlock (&lcm->mutex);
if (buf_avail < lcm->udp_low_watermark)
lcm->udp_low_watermark = buf_avail;
GTimeVal tv;
g_get_current_time(&tv);
int elapsedsecs = tv.tv_sec - lcm->udp_last_report_secs;
if (elapsedsecs > 2) {
uint32_t total_bad = lcm->udp_discarded_lcmb +
lcm->udp_discarded_buf +
lcm->udp_discarded_bad;
if (total_bad > 0 || lcm->udp_low_watermark < 0.5) {
fprintf(stderr,
"%d.%03d LCM loss %4.1f%% : %5d lcmb, %5d buf, %5d err, "
"buf avail %4.1f%%\n",
(int) tv.tv_sec, (int) tv.tv_usec/1000,
total_bad * 100.0 / (lcm->udp_rx + total_bad),
lcm->udp_discarded_lcmb,
lcm->udp_discarded_buf,
lcm->udp_discarded_bad,
100.0 * lcm->udp_low_watermark);
lcm->udp_rx = 0;
lcm->udp_discarded_lcmb = 0;
lcm->udp_discarded_buf = 0;
lcm->udp_discarded_bad = 0;
lcm->udp_last_report_secs = tv.tv_sec;
lcm->udp_low_watermark = HUGE;
}
}
int got_complete_message = 0;
while (!got_complete_message) {
// wait for either incoming UDP data, or for an abort message
fd_set fds;
FD_ZERO (&fds);
FD_SET (lcm->recvfd, &fds);
FD_SET (lcm->thread_msg_pipe[0], &fds);
SOCKET maxfd = MAX(lcm->recvfd, lcm->thread_msg_pipe[0]);
if (select (maxfd + 1, &fds, NULL, NULL, NULL) <= 0) {
perror ("udp_read_packet -- select:");
continue;
}
if (FD_ISSET (lcm->thread_msg_pipe[0], &fds)) {
// received an exit command.
dbg (DBG_LCM, "read thread received exit command\n");
if (lcmb) {
// lcmb is not on one of the memory managed buffer queues. We could
// either put it back on one of the queues, or just free it here. Do the
// latter.
//
// Can also just free its lcm_buf_t here. Its data buffer is
// managed either by the ring buffer or the fragment buffer, so
// we can ignore it.
free (lcmb);
}
return NULL;
}
// there is incoming UDP data ready.
assert (FD_ISSET (lcm->recvfd, &fds));
if (!lcmb) {
// try to allocate space on the ringbuffer for the new data
// first allocate a buffer struct
g_static_rec_mutex_lock (&lcm->mutex);
lcmb = lcm_buf_dequeue (lcm->inbufs_empty);
if (!lcmb) {
g_static_rec_mutex_unlock (&lcm->mutex);
udp_discard_packet (lcm);
continue;
}
lcmb->buf_from_ringbuf = 1;
// next allocate space on the ringbuffer.
// give it the maximum possible size for an unfragmented packet
lcmb->buf = lcm_ringbuf_alloc(lcm->ringbuf, 65536);
g_static_rec_mutex_unlock (&lcm->mutex);
if (!lcmb->buf) {
// ringbuffer is full. discard the packet, put lcmb back on the
// empty queue, and start waiting again
udp_discard_packet (lcm);
lcm_buf_enqueue (lcm->inbufs_empty, lcmb);
lcmb = NULL;
continue;
}
// zero the last byte so that strlen never segfaults
lcmb->buf[65535] = 0;
}
struct iovec vec;
vec.iov_base = lcmb->buf;
vec.iov_len = 65535;
#ifdef MSG_EXT_HDR
// operating systems that provide SO_TIMESTAMP allow us to obtain more
// accurate timestamps by having the kernel produce timestamps as soon
// as packets are received.
char controlbuf[64];
#endif
struct msghdr msg;
msg.msg_name = &lcmb->from;
msg.msg_namelen = sizeof (struct sockaddr);
msg.msg_iov = &vec;
msg.msg_iovlen = 1;
#ifdef MSG_EXT_HDR
msg.msg_control = controlbuf;
msg.msg_controllen = sizeof (controlbuf);
msg.msg_flags = 0;
#endif
sz = recvmsg (lcm->recvfd, &msg, 0);
if (sz < 0) {
perror ("udp_read_packet -- recvmsg");
lcm->udp_discarded_bad++;
continue;
}
if (sz < sizeof(lcm2_header_short_t)) {
// packet too short to be LCM
lcm->udp_discarded_bad++;
continue;
}
lcmb->fromlen = msg.msg_namelen;
int got_utime = 0;
#ifdef SO_TIMESTAMP
struct cmsghdr * cmsg = CMSG_FIRSTHDR (&msg);
/* Get the receive timestamp out of the packet headers if possible */
while (!lcmb->recv_utime && cmsg) {
if (cmsg->cmsg_level == SOL_SOCKET &&
cmsg->cmsg_type == SCM_TIMESTAMP) {
// GTimeVal is identical to struct timeval, so this cast is ok
GTimeVal * t = (GTimeVal*) CMSG_DATA (cmsg);
lcmb->recv_utime = (int64_t) t->tv_sec * 1000000 + t->tv_usec;
got_utime = 1;
break;
}
cmsg = CMSG_NXTHDR (&msg, cmsg);
}
#endif
if (!got_utime)
lcmb->recv_utime = _timestamp_now ();
lcm2_header_short_t *hdr2 = (lcm2_header_short_t*) lcmb->buf;
uint32_t rcvd_magic = ntohl(hdr2->magic);
if (rcvd_magic == LCM2_MAGIC_SHORT)
got_complete_message = _recv_short_message (lcm, lcmb, sz);
else if (rcvd_magic == LCM2_MAGIC_LONG)
got_complete_message = _recv_message_fragment (lcm, lcmb, sz);
else {
dbg (DBG_LCM, "LCM: bad magic\n");
lcm->udp_discarded_bad++;
continue;
}
}
// if the newly received packet is a short packet, then resize the space
// allocated to it on the ringbuffer to exactly match the amount of space
// required. That way, we do not use 64k of the ringbuffer for every
// incoming message.
if (lcmb->buf_from_ringbuf) {
g_static_rec_mutex_lock (&lcm->mutex);
lcm_ringbuf_shrink_last(lcm->ringbuf, lcmb->buf, sz);
g_static_rec_mutex_unlock (&lcm->mutex);
}
return lcmb;
}
int main(int argc, char **argv)
{
g_type_init();
CamUnit *resize_unit = NULL;
CamUnit *faulty_unit = NULL;
CamUnitChain * chain = NULL;
const char *input_id = NULL;
state_t s;
memset (&s, 0, sizeof (s));
sprintf (s.filename, "snapshot-output.ppm");
// create the GLib mainloop
s.mainloop = g_main_loop_new (NULL, FALSE);
s.lcm = lcm_create ("udpm://?transmit_only=true");
if (!s.lcm) {
fprintf (stderr, "Couldn't create LCM\n");
goto failed;
}
// create the image processing chain
chain = cam_unit_chain_new ();
// abort if no input unit was specified
if (argc < 2) {
print_usage_and_inputs (argv[0], chain->manager);
goto failed;
}
input_id = argv[1];
// instantiate the input unit
if (! cam_unit_chain_add_unit_by_id (chain, input_id)) {
fprintf (stderr, "Oh no! Couldn't create input unit [%s].\n\n",
input_id);
print_usage_and_inputs (argv[0], chain->manager);
goto failed;
}
// create a unit to convert the input data to 8-bit RGB
cam_unit_chain_add_unit_by_id (chain, "convert.to_rgb8");
resize_unit = cam_unit_chain_add_unit_by_id (chain,
"filter.ipp.resize");
// cam_unit_set_control_boolean (resize_unit, "aspect-lock", FALSE);
cam_unit_set_control_int (resize_unit, "width", 64);
// cam_unit_set_control_int (resize_unit, "height", 64);
// start the image processing chain
faulty_unit = cam_unit_chain_all_units_stream_init (chain);
// did everything start up correctly?
if (faulty_unit) {
fprintf (stderr, "Unit [%s] is not streaming, aborting...\n",
cam_unit_get_name (faulty_unit));
goto failed;
}
// attach the chain to the glib event loop.
cam_unit_chain_attach_glib (chain, 1000, NULL);
// subscribe to be notified when an image has made its way through the
// chain
g_signal_connect (G_OBJECT (chain), "frame-ready",
G_CALLBACK (on_frame_ready), &s);
s.next_publish_utime = _timestamp_now ();
// run
g_main_loop_run (s.mainloop);
// cleanup
g_main_loop_unref (s.mainloop);
cam_unit_chain_all_units_stream_shutdown (chain);
g_object_unref (chain);
return 0;
failed:
g_main_loop_unref (s.mainloop);
cam_unit_chain_all_units_stream_shutdown (chain);
g_object_unref (chain);
return 1;
}
