/********************************************************* * * This source code is part of the Carnegie Mellon Robot * Navigation Toolkit (CARMEN) * * CARMEN Copyright (c) 2002 Michael Montemerlo, Nicholas * Roy, Sebastian Thrun, Dirk Haehnel, Cyrill Stachniss, * and Jared Glover * * CARMEN is free software; you can redistribute it and/or  * modify it under the terms of the GNU General Public  * License as published by the Free Software Foundation;  * either version 2 of the License, or (at your option) * any later version. * * CARMEN is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR * PURPOSE.  See the GNU General Public License for more  * details. * * You should have received a copy of the GNU General  * Public License along with CARMEN; if not, write to the * Free Software Foundation, Inc., 59 Temple Place,  * Suite 330, Boston, MA  02111-1307 USA * ********************************************************/#include <stdlib.h>#include <stdio.h>#include <pthread.h>#include <signal.h>#include <string.h>#include <errno.h>#include <unistd.h>#include <stdint.h>#include <stdarg.h>#include "orc.h"#include "packet.h"#include "timespec.h"#include "clog.h"#include "log.h"#include "orcconstants.h"static uint8_t garbage[256];#define VPRINTALLOC                                                \	int sz = 4096;						   \	char *buf = NULL;                                          \	_revprintf:                                                \        buf = (char*) realloc(buf,sz);	               		   \	va_list ap;                                                \	int len;                                                   \	va_start(ap, fmt);                                         \	len = vsnprintf(buf, sz-1, fmt, ap);                       \	if (len < 0 || len>=sz)                                    \            { sz*=2; goto _revprintf; }                            \	va_end(ap);                                                static ssize_t read_fully(int fd, uint8_t *buf, ssize_t count){        ssize_t readsofar = 0;        ssize_t readthistime;        while (readsofar < count) {                readthistime = read(fd, &buf[readsofar], count - readsofar);                if (readthistime < 0) {			LOG_ERROR("ERROR: read failed with %ld (%s)\n", (long) readthistime, strerror(errno));                        return -1; // error!                }                if (readthistime == 0) {                        LOG_ERROR("ERROR: got EOF while reading %ld\n", (long) readthistime);                        return -1; // that's an error for us.                }                readsofar += readthistime;        }        return readsofar;}static ssize_t write_fully(int fd, uint8_t *buf, ssize_t count){        ssize_t writtensofar = 0;        ssize_t writtenthistime;        while (writtensofar < count) {                writtenthistime = write(fd, &buf[writtensofar], count - writtensofar);                if (writtenthistime <= 0)                        return -1;                writtensofar += writtenthistime;        }        return writtensofar;}static void setup_thread(){	int ot;	pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,&ot);	signal(SIGPIPE, SIG_IGN);}static void handle_heartbeat_packet(orc_t *orc, uint8_t *p){	orc->heartbeat_time = packet_16u(p, 3);	orc->heartbeat_flags = packet_8u(p, 7);}static void handle_pad_packet(orc_t *orc, uint8_t *p){	orc->pad_switches = p[PACKET_DATA + 1];	orc->pad_updown = p[PACKET_DATA + 2];	orc->pad_leftright = p[PACKET_DATA + 3];	orc->pad_joyx = p[PACKET_DATA + 4];	orc->pad_joyy = p[PACKET_DATA + 5];	pthread_mutex_lock(&orc->pad_mutex);	pthread_cond_broadcast(&orc->pad_cond);	pthread_mutex_unlock(&orc->pad_mutex);}void *reader_thread(void *_arg){	orc_t *orc = (orc_t*) _arg;	setup_thread();	orc->fd = -1;	int reconnectcount = 0;reconnect:		// reconnect, if necessary.	while (orc->fd < 0) {		LOG_INFO("Trying to connect to orcboard...(%i)", reconnectcount++);		orc->fd = orc->impl->connect(orc->impl);				if (orc->fd < 0)			sleep(1);	}	// read for a while	while (1) {		// read a packet		uint8_t buf[3];		int res = read_fully(orc->fd, buf, 1);		if (res <= 0)			goto disconnected;		if (buf[0] != 0xED) {			LOG_DEBUG("Recovering sync [%02X]", buf[0]);			continue;		}		res = read_fully(orc->fd, &buf[1], 2);		if (res <= 0)			goto disconnected;		int id = buf[1];		int datalen = buf[2];		transaction_t *t = &orc->transactions[id];		memcpy(t->response, buf, 3);		res = read_fully(orc->fd, &t->response[PACKET_DATA], datalen + 1);		if (res <= 0)			goto disconnected;		if (!packet_test_checksum(t->response)) {			LOG_WARN("Bad checksum received from Orc");			continue;		}		if (t->response == garbage && t->response[1]>=orc->idLow && t->response[1]<=orc->idHigh)			LOG_VERBOSE("Unsolicited ack, id = %02X", t->response[1]);		// is this a message from orcd, assigning us a client id?		if (t->response[1] == 0xf7) {			orc->idLow = t->response[4];			orc->idHigh = t->response[5];			orc->idLast = orc->idLow;			LOG_INFO("Got client transaction range: %02x-%02x", orc->idLow, orc->idHigh);		}		if (t->response[1] == 0xfe)			handle_pad_packet(orc, t->response);		if (t->response[1] == PACKET_ID_ORCBOARD_BROADCAST &&		    packet_16u(t->response, 1) == MSG_ASYNC_HEARTBEAT)			handle_heartbeat_packet(orc, t->response);		pthread_mutex_lock(&t->mutex);		pthread_cond_signal(&t->cond);		pthread_mutex_unlock(&t->mutex);	}	disconnected:	orc->impl->disconnect(orc->impl, orc->fd);	orc->fd = -1;	goto reconnect;	// silence compiler	return NULL;}orc_t *orc_create(orc_comms_impl_t *impl){	orc_t *orc = (orc_t*) calloc(sizeof(orc_t), 1);	orc->impl = impl;	pthread_attr_t threadAttr;	pthread_attr_init(&threadAttr);	pthread_attr_setstacksize(&threadAttr, 32768);	pthread_t newthread;	if (pthread_create(&newthread, &threadAttr, reader_thread, orc)) {		LOG_ERROR("Couldn't create orc reader thread: %s", strerror(errno));		exit(EXIT_FAILURE);	}	for (int i = 0; i < 256; i++) {		pthread_mutex_init(&orc->transactions[i].mutex, NULL);		pthread_cond_init(&orc->transactions[i].cond, NULL);		orc->transactions[i].inuse = 0;		orc->transactions[i].response = garbage;	}	pthread_mutex_init(&orc->writeLock, NULL);	pthread_mutex_init(&orc->idMutex, NULL);	pthread_cond_init(&orc->idCond, NULL);	orc->idUsed = 0;	orc->idLow = 0x00; // initially, we can use any id	orc->idHigh = 0xef;	orc->idLast = orc->idLow;	pthread_mutex_init(&orc->pad_mutex, NULL);	pthread_cond_init(&orc->pad_cond, NULL);	orc_null(orc, 0, 0);	return orc;}void orc_destroy(orc_t *orc){	free(orc);}static int alloc_transaction_id(orc_t *orc){	int id;	pthread_mutex_lock(&orc->idMutex);	// wait for a transaction to become available	int max = orc->idHigh - orc->idLow + 1;	while (orc->idUsed >= max)		pthread_cond_wait(&orc->idCond, &orc->idMutex);			// search for a transaction id, beginning our search at idLast	do {		orc->idLast++;		if (orc->idLast > orc->idHigh)			orc->idLast = orc->idLow;	} while (orc->transactions[orc->idLast].inuse);	// initialize the transaction	id = orc->idLast;		orc->idUsed++;	pthread_mutex_unlock(&orc->idMutex);	return id;}static void free_transaction_id(orc_t *orc, int id){	pthread_mutex_lock(&orc->idMutex);	orc->transactions[id].inuse = 0;	orc->idUsed--;	pthread_cond_signal(&orc->idCond);	pthread_mutex_unlock(&orc->idMutex);}int orc_transaction_once(orc_t *orc, uint8_t *request, uint8_t *response){	while (orc->fd <= 0) {		usleep(1000);	}		// grab an id	int id = alloc_transaction_id(orc);	// fix up the packet.	request[0] = PACKET_MARKER;	request[PACKET_ID] = id;	packet_fill_checksum(request);	// fill out the transaction record	transaction_t *t = &orc->transactions[id];	pthread_mutex_lock(&t->mutex);	t->response = response;	// send the packet	pthread_mutex_lock(&orc->writeLock);	write_fully(orc->fd, request, request[PACKET_DATALEN] + 4);	pthread_mutex_unlock(&orc->writeLock);	// compute our timeout time	struct timespec ts;	timespec_now(&ts);	timespec_addms(&ts, 100);		// wait!	int res = pthread_cond_timedwait(&t->cond, &t->mutex, &ts);	t->response = garbage;		// cleanup	pthread_mutex_unlock(&t->mutex);	free_transaction_id(orc, id);	if (res == ETIMEDOUT) {		LOG_VERBOSE("Timeout packet, id = %02X", id);		return -1;	}	return 0;}void orc_transaction_async(orc_t *orc, uint8_t *request){	// fix up the packet.	request[0] = PACKET_MARKER;