Commit d286c0cb authored by Timothy Stack's avatar Timothy Stack

Start on vmc:

	* configure, configure.in: Add vmcd related template files.

	* robots/GNUmakefile.in: Switch order of vmcd/rmcd.

	* robots/emc/GNUmakefile.in: cleanup

	* robots/mtp/GNUmakefile.in: Add mtp_dump tool.

	* robots/mtp/mtp.c: Change mtp_encode_packet to use a passed in
	buffer pointer or allocate a buffer if its NULL, probably gonna be
	a big source of errors...

	* robots/mtp/mtp_dump.c: Another command-line tool that connects
	to a server and dumps mtp packets that are received.  Useful for
	seeing output from the vmc-client.

	* robots/vmcd/GNUmakefile.in: Add vmc-client and test case.

	* robots/vmcd/test_vmc-client.sh.in: Test case for the vmc-client.

	* robots/vmcd/vmc-client.c: First cut of the vmc-client, it reads
	mezzanine output and sends it to any connected clients.
parent 0347520d
......@@ -1584,7 +1584,8 @@ outfiles="$outfiles Makeconf GNUmakefile \
install/newnode_sshkeys/GNUmakefile \
mote/GNUmakefile mote/tbuisp \
robots/GNUmakefile robots/mtp/GNUmakefile robots/emc/GNUmakefile \
robots/emc/test_emcd.sh robots/emc/loclistener "
robots/emc/test_emcd.sh robots/emc/loclistener \
robots/vmcd/GNUmakefile robots/vmcd/test_vmc-client.sh "
#
# Do this for easy distclean.
......
......@@ -614,7 +614,8 @@ outfiles="$outfiles Makeconf GNUmakefile \
install/newnode_sshkeys/GNUmakefile \
mote/GNUmakefile mote/tbuisp \
robots/GNUmakefile robots/mtp/GNUmakefile robots/emc/GNUmakefile \
robots/emc/test_emcd.sh robots/emc/loclistener "
robots/emc/test_emcd.sh robots/emc/loclistener \
robots/vmcd/GNUmakefile robots/vmcd/test_vmc-client.sh "
#
# Do this for easy distclean.
......
......@@ -11,7 +11,7 @@ SUBDIR = robots
include $(OBJDIR)/Makeconf
SUBDIRS = mtp emc rmcd vmcd
SUBDIRS = mtp emc vmcd rmcd
all: all-subdirs
check: check-subdirs
......
......@@ -18,8 +18,8 @@ all: $(PROGS)
include $(TESTBED_SRCDIR)/GNUmakerules
OBJS = emcd.o
CFLAGS += -O -g -Wall -I${OBJDIR} -I/usr/local/include
OBJS = emcd.o robot_list.o
CFLAGS += -O -g -Wall -I${OBJDIR} -I/usr/local/include
CFLAGS += -I${SRCDIR}/../mtp -I${SRCDIR}/../../event/lib
CFLAGS += -I${SRCDIR}/../../lib/libtb
CFLAGS += `elvin-config --cflags vin4c`
......@@ -30,8 +30,8 @@ LIBS += `elvin-config --libs vin4c`
test_emcd.sh: emcd
emcd: emcd.o robot_list.o ../mtp/libmtp.a
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ emcd.o robot_list.o $(LIBS)
emcd: $(OBJS) ../mtp/libmtp.a
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $(OBJS) $(LIBS)
install: all
-mkdir -p $(INSTALL_DIR)/opsdir/sbin
......
......@@ -12,7 +12,7 @@ SUBDIR = robots/mtp
include $(OBJDIR)/Makeconf
MTPLIBS = libmtp.a
MTPPROGS = mtp_test mtp_send mtp_recv
MTPPROGS = mtp_test mtp_send mtp_recv mtp_dump
TESTS = mtp_test
......@@ -31,6 +31,7 @@ mtp.o: mtp.h
mtp_test.o: mtp.h
mtp_send.o: mtp.h
mtp_recv.o: mtp.h
mtp_dump.o: mtp.h
mtp_test: mtp_test.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_test.o -L. -lmtp
......@@ -41,10 +42,14 @@ mtp_send: mtp_send.o $(MTPLIBS)
mtp_recv: mtp_recv.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_recv.o -L. -lmtp
mtp_dump: mtp_dump.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_dump.o -L. -lmtp
install: all
-mkdir -p $(INSTALL_DIR)/opsdir/bin
$(INSTALL_PROGRAM) mtp_send $(INSTALL_DIR)/opsdir/bin/mtp_send
$(INSTALL_PROGRAM) mtp_recv $(INSTALL_DIR)/opsdir/bin/mtp_recv
$(INSTALL_PROGRAM) mtp_dump $(INSTALL_DIR)/opsdir/bin/mtp_dump
clean:
/bin/rm -f *.o *.a $(MTPPROGS)
......@@ -106,7 +106,7 @@ int mtp_receive_packet(int fd,struct mtp_packet **packet) {
}
int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
char *buf;
char *buf = *buf_ptr;
int i,j;
int buf_size;
......@@ -119,19 +119,24 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
// but we're encoding variable-length arrays and strings in addition
// to ints and floats -- but we didn't -- so we have to skim through.
if (packet->opcode == MTP_CONTROL_ERROR ||
packet->opcode == MTP_CONTROL_NOTIFY ||
packet->opcode == MTP_CONTROL_INIT ||
packet->opcode == MTP_CONTROL_CLOSE) {
struct mtp_control *data = packet->data.control;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
if ((buf_size = mtp_calc_size(packet->opcode,
(void *)packet->data.control)) == -1) {
return MTP_PP_ERROR;
}
if (buf == NULL) {
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
}
if (packet->opcode == MTP_CONTROL_ERROR ||
packet->opcode == MTP_CONTROL_NOTIFY ||
packet->opcode == MTP_CONTROL_INIT ||
packet->opcode == MTP_CONTROL_CLOSE) {
struct mtp_control *data = packet->data.control;
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -151,13 +156,6 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
}
else if (packet->opcode == MTP_CONFIG_RMC) {
struct mtp_config_rmc *data = packet->data.config_rmc;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -183,13 +181,6 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
}
else if (packet->opcode == MTP_CONFIG_VMC) {
struct mtp_config_rmc *data = packet->data.config_rmc;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -213,13 +204,6 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
}
else if (packet->opcode == MTP_REQUEST_POSITION) {
struct mtp_request_position *data = packet->data.request_position;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -234,13 +218,6 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
}
else if (packet->opcode == MTP_REQUEST_ID) {
struct mtp_request_id *data = packet->data.request_id;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -258,13 +235,6 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
}
else if (packet->opcode == MTP_UPDATE_POSITION) {
struct mtp_update_position *data = packet->data.update_position;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -286,13 +256,6 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
}
else if (packet->opcode == MTP_UPDATE_ID) {
struct mtp_update_id *data = packet->data.update_id;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -307,13 +270,6 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
}
else if (packet->opcode == MTP_COMMAND_GOTO) {
struct mtp_command_goto *data = packet->data.command_goto;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -333,13 +289,6 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
}
else if (packet->opcode == MTP_COMMAND_STOP) {
struct mtp_command_stop *data = packet->data.command_stop;
buf_size = mtp_calc_size(packet->opcode,(void *)data);
buf = (char *)malloc(sizeof(char)*buf_size);
*buf_ptr = buf;
if (buf == NULL) {
return MTP_PP_ERROR_MALLOC;
}
*((int *)buf) = htonl(buf_size);
buf[MTP_PACKET_HEADER_OFFSET_OPCODE] = (char)(packet->opcode);
......@@ -541,7 +490,7 @@ int mtp_decode_packet(char *buf,struct mtp_packet **packet_ptr) {
}
int mtp_send_packet(int fd,struct mtp_packet *packet) {
char *buf;
char *buf = NULL;
int retval;
if (packet == NULL) {
......
#include "config.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <sys/types.h>
#include <netdb.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include "mtp.h"
static int mygethostbyname(struct sockaddr_in *host_addr, char *host)
{
struct hostent *host_ent;
int retval = 0;
assert(host_addr != NULL);
assert(host != NULL);
assert(strlen(host) > 0);
if( (host_ent = gethostbyname(host)) != NULL ) {
memcpy((char *)&host_addr->sin_addr.s_addr,
host_ent->h_addr,
host_ent->h_length);
retval = 1;
}
else {
retval = inet_aton(host, &host_addr->sin_addr);
}
return( retval );
}
static void usage(void)
{
fprintf(stderr, "Usage: mtp_dump <host> <port>\n");
}
int main(int argc, char *argv[])
{
int port = 0, max_packets = 0, retval = EXIT_FAILURE;
struct sockaddr_in sin;
memset(&sin, 0, sizeof(sin));
if (argc < 3) {
fprintf(stderr, "error: not enough arguments\n");
usage();
}
else if (sscanf(argv[2], "%d", &port) != 1) {
fprintf(stderr, "error: port is not a number\n");
}
else if ((argc > 3) && sscanf(argv[3], "%d", &max_packets) != 1) {
fprintf(stderr, "error: max packets argument is not a number\n");
}
else if (mygethostbyname(&sin, argv[1]) == 0) {
fprintf(stderr, "error: unknown host %s\n", argv[1]);
}
else {
int fd;
sin.sin_len = sizeof(sin);
sin.sin_family = AF_INET;
sin.sin_port = htons(port);
if ((fd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
perror("socket");
retval = EXIT_FAILURE;
}
else if (connect(fd, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
perror("connect");
}
else {
struct mtp_packet *mp;
int lpc = 0;
while (((max_packets == 0) || (lpc < max_packets)) &&
(mtp_receive_packet(fd, &mp) == MTP_PP_SUCCESS)) {
mtp_print_packet(stdout, mp);
mtp_free_packet(mp);
mp = NULL;
lpc += 1;
}
close(fd);
fd = -1;
}
retval = EXIT_SUCCESS;
}
return retval;
}
......@@ -11,9 +11,25 @@ SUBDIR = robots/vmcd
include $(OBJDIR)/Makeconf
all:
PROGS = vmc-client
TESTS = test_vmc-client.sh
all: $(PROGS)
include $(TESTBED_SRCDIR)/GNUmakerules
CFLAGS += -O -g -Wall -I${OBJDIR} -I/usr/local/include
CFLAGS += -I${SRCDIR}/../mtp
CFLAGS += -I${SRCDIR}/../../lib/libtb
LDFLAGS = -L../mtp -L${OBJDIR}/lib/libtb -L${OBJDIR}/event/lib
LIBS += -lmtp -ltb
test_vmc-client.sh: vmc-client
vmc-client: vmc-client.o
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $< $(LIBS)
clean:
rm -f *.o
#! /bin/sh
## Variables
# The full path of the test case
test_file=$1
# The base name of the test case
test_file_base="test_vmc-client.sh"
# The current test number for shell based tests.
test_num=0
SRCDIR=@srcdir@
PORT=7070
## Helper functions
run_test() {
echo "run_test: $*"
$* > ${test_file_base}_${test_num}.tmp 2>&1
}
check_output() {
diff -u - ${test_file_base}_${test_num}.tmp
if test $? -ne 0; then
echo $1
exit 1
fi
test_num=`expr ${test_num} \+ 1`
}
##
vmc-client -l `pwd`/test_vmc-client.log \
-i `pwd`/test_vmc-client.pid \
-p ${PORT} \
foobar
trap 'kill `cat test_vmc-client.pid`' EXIT
sleep 1
(sleep 1; kill -s USR1 `cat test_vmc-client.pid`) &
run_test ../mtp/mtp_dump localhost ${PORT} 2
check_output "bad update?" <<EOF
Packet: length 31; version 1; role 2
opcode: update-position
id: -1
x: 2.500000
y: 5.500000
theta: 0.480000
status: -1
timestamp: 20
Packet: length 31; version 1; role 2
opcode: update-position
id: -1
x: 4.500000
y: 6.500000
theta: 0.540000
status: -1
timestamp: 20
EOF
#include <stdio.h>
#include <errno.h>
#include <assert.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include "log.h"
#include "mtp.h"
#if defined(HAVE_MEZZANINE)
#include "mezz.h"
#else
#define MEZZ_MAX_OBJECTS 100
typedef struct {
int class[2]; // Color class for the two blobs
double max_disp; // Maximum inter-frame displacement
double max_sep; // Maximum blob separation
int max_missed; // Max frames before looking for a new match
int missed; // Number of missed frames (object not seen)
double px, py, pa; // Object pose (world cs).
} mezz_object_t;
typedef struct {
int count;
mezz_object_t objects[MEZZ_MAX_OBJECTS];
} mezz_objectlist_t;
typedef struct {
double time;
mezz_objectlist_t objectlist;
} mezz_mmap_t;
#endif
#define VMCCLIENT_DEFAULT_PORT 6969
static int debug = 0;
static int looping = 1;
static int mezz_event_count = 0;
static void usage(void)
{
fprintf(stderr,
"Usage: vmcclient [-hd] [-p port] mezzfile\n"
"Required arguments:\n"
" mezzfile\tThe name of the mezzanine shared file\n"
"Options:\n"
" -h\t\tPrint this message\n"
" -d\t\tTurn on debugging messages and do not daemonize\n"
" -l logfile\tSpecify the log file\n"
" -p port\tSpecify the port number to listen on. (Default: %d)\n",
VMCCLIENT_DEFAULT_PORT);
}
static void sigquit(int signal)
{
looping = 0;
}
static void sigusr1(int signal)
{
mezz_event_count += 1;
}
static int encode_packets(char *buffer, mezz_mmap_t *mm)
{
struct mtp_update_position mup;
mezz_objectlist_t *mol;
struct mtp_packet mp;
int lpc, retval;
char *cursor;
assert(buffer != NULL);
assert(mm != NULL);
mol = &mm->objectlist;
mp.length = mtp_calc_size(MTP_UPDATE_POSITION, &mup);
mp.opcode = MTP_UPDATE_POSITION;
mp.version = MTP_VERSION;
mp.role = MTP_ROLE_VMC;
mp.data.update_position = &mup;
cursor = buffer;
for (lpc = 0; lpc < mol->count; lpc++) {
mup.robot_id = -1;
mup.position.x = mol->objects[lpc].px;
mup.position.y = mol->objects[lpc].py;
mup.position.theta = mol->objects[lpc].pa;
mup.status = -1;
mup.timestamp = mm->time;
cursor += mtp_encode_packet(&cursor, &mp);
}
retval = cursor - buffer;
return retval;
}
int main(int argc, char *argv[])
{
int c, port = VMCCLIENT_DEFAULT_PORT, serv_sock, on_off = 1;
char *mezzfile, *logfile = NULL, *pidfile = NULL;
mezz_mmap_t *mezzmap = NULL;
int retval = EXIT_FAILURE;
struct sockaddr_in sin;
struct sigaction sa;
while ((c = getopt(argc, argv, "hdp:l:i:")) != -1) {
switch (c) {
case 'h':
usage();
exit(0);
break;
case 'd':
debug += 1;
break;
case 'l':
logfile = optarg;
break;
case 'i':
pidfile = optarg;
break;
case 'p':
if (sscanf(optarg, "%d", &port) != 1) {
error("-p option is not a number: %s\n", optarg);
usage();
exit(1);
}
break;
default:
break;
}
}
argv += optind;
argc -= optind;
if (argc == 0) {
error("missing mezzanine file argument\n");
usage();
exit(1);
}
signal(SIGQUIT, sigquit);
signal(SIGTERM, sigquit);
signal(SIGINT, sigquit);
sa.sa_handler = sigusr1;
sigemptyset(&sa.sa_mask);
sa.sa_flags = 0;
sigaction(SIGUSR1, &sa, NULL);
mezzfile = argv[0];
if (debug) {
loginit(0, logfile);
}
else {
/* Become a daemon */
daemon(0, 0);
if (logfile)
loginit(0, logfile);
else
loginit(1, "vmcclient");
}
#if defined(HAVE_MEZZANINE)
{
if (mezz_init(0, mezzfile) == -1) {
errorc("unable to initialize mezzanine\n");
exit(2);
}
mezzmap = mezz_mmap();
}
#else
{
mezzmap = calloc(1, sizeof(mezz_mmap_t));
mezzmap->time = 20.0;
mezzmap->objectlist.count = 2;
mezzmap->objectlist.objects[0].px = 2.5;
mezzmap->objectlist.objects[0].py = 5.5;
mezzmap->objectlist.objects[0].pa = 0.48;
mezzmap->objectlist.objects[1].px = 4.5;
mezzmap->objectlist.objects[1].py = 6.5;
mezzmap->objectlist.objects[1].pa = 0.54;
}
#endif
if (pidfile) {
FILE *fp;
if ((fp = fopen(pidfile, "w")) != NULL) {
fprintf(fp, "%d\n", getpid());
(void) fclose(fp);
}
}
memset(&sin, 0, sizeof(sin));
sin.sin_len = sizeof(sin);
sin.sin_family = AF_INET;
sin.sin_port = htons(port);
sin.sin_addr.s_addr = INADDR_ANY;
if ((serv_sock = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
error("cannot create socket\n");
}
else if (setsockopt(serv_sock,
SOL_SOCKET,
SO_REUSEADDR,
&on_off,
sizeof(on_off)) == -1) {
errorc("setsockopt");
}
else if (bind(serv_sock, (struct sockaddr *)&sin, sizeof(sin)) == -1) {
errorc("bind");
}
else if (listen(serv_sock, 5) == -1) {
errorc("listen");
}
else {
fd_set readfds, clientfds;
int last_mezz_event = 0;
FD_ZERO(&readfds);
FD_ZERO(&clientfds);
FD_SET(serv_sock, &readfds);
while (looping) {
int rc;
rc = select(FD_SETSIZE, &readfds, NULL, NULL, NULL);
if (rc > 0) {
int lpc;
if (FD_ISSET(serv_sock, &readfds)) {
struct sockaddr_in peer_sin;
socklen_t slen;
int fd;
slen = sizeof(peer_sin);
if ((fd = accept(serv_sock,
(struct sockaddr *)&peer_sin,
&slen)) == -1) {
errorc("accept");
}
else {
FD_SET(fd, &readfds);
FD_SET(fd, &clientfds);
}
}
for (lpc = 0; lpc < rc; lpc++) {
if (FD_ISSET(lpc, &readfds)) {
close(lpc);
FD_CLR(lpc, &readfds);
FD_CLR(lpc, &clientfds);
}
}
}
else if (rc == -1) {
switch (errno) {
case EINTR:
if (mezz_event_count > last_mezz_event) {
char buffer[2048];
if (debug) {
info("sending updates\n");
}
if ((rc = encode_packets(buffer, mezzmap)) == -1) {
errorc("unable to encode packets");
}
else {
int lpc;
for (lpc = 0; lpc < FD_SETSIZE; lpc++) {
if (FD_ISSET(lpc, &clientfds)) {
write(lpc, buffer, rc);
}
}
}
last_mezz_event = mezz_event_count;
}
break;
default:
errorc("unhandled select error\n");
break;
}
}
}
}
#if defined(HAVE_MEZZANINE)