Commit feea9720 authored by Timothy Stack's avatar Timothy Stack
Browse files

Elvinize emc and some bug fixes...

	* configure, configure.in: Add "robots/emc/test_emcd.sh" script to
	the list of template files.

	* robots/GNUmakefile.in: Add a target for install-subdir.

	* robots/emc/GNUmakefile.in: Compile emcd and install it on ops.
	Add test_emcd.sh test case.

	* robots/emc/emcd.h, robots/emc/emcd.c: Elvinize, add support for
	events, and some minor cleanup.

	* robots/emc/robot_list.c: Compilation fixes.

	* robots/emc/test_emcd.config: Robot config for the test case.

	* robots/emc/test_emcd.sh.in: Test case for emcd, just starts it
	up and uses mtp_send to send a few messages to it.

	* robots/mtp/GNUmakefile.in: Install mtp_send and mtp_recv on ops.

	* robots/mtp/mtp.h, robots/mtp/mtp.c: Marshall floats correctly,
	doh!  Move the packet printing code from mtp_recv to the lib.

	* robots/mtp/mtp_recv.c: Move the packet printing code to the
	lib.

	* robots/mtp/mtp_send.c: Add a "-w" option to wait for a response
	from the peer and then dump the packet to stdout.  Allow multiple
	packets to be sent from a single invocation, the arguments for
	each packet must be separated by a double dash (--), see
	test_emcd.sh.in for an example.

	* robots/mtp/mtp_test.c: Gah, test with actual floating point
	values dummy.
parent 2ea12718
......@@ -1583,7 +1583,8 @@ outfiles="$outfiles Makeconf GNUmakefile \
install/GNUmakefile install/ops-install install/boss-install \
install/newnode_sshkeys/GNUmakefile \
mote/GNUmakefile mote/tbuisp \
robots/GNUmakefile robots/mtp/GNUmakefile "
robots/GNUmakefile robots/mtp/GNUmakefile robots/emc/GNUmakefile \
robots/emc/test_emcd.sh "
#
# Do this for easy distclean.
......
......@@ -613,7 +613,8 @@ outfiles="$outfiles Makeconf GNUmakefile \
install/GNUmakefile install/ops-install install/boss-install \
install/newnode_sshkeys/GNUmakefile \
mote/GNUmakefile mote/tbuisp \
robots/GNUmakefile robots/mtp/GNUmakefile "
robots/GNUmakefile robots/mtp/GNUmakefile robots/emc/GNUmakefile \
robots/emc/test_emcd.sh "
#
# Do this for easy distclean.
......
......@@ -15,6 +15,7 @@ SUBDIRS = mtp emc rmcd vmcd
all: all-subdirs
check: check-subdirs
install: install-subdirs
include $(TESTBED_SRCDIR)/GNUmakerules
......
......@@ -11,9 +11,31 @@ SUBDIR = robots/emc
include $(OBJDIR)/Makeconf
all:
PROGS = emcd
TESTS = test_emcd.sh
all: $(PROGS)
include $(TESTBED_SRCDIR)/GNUmakerules
OBJS = emcd.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`
LDFLAGS = -L../mtp -L${OBJDIR}/lib/libtb -L${OBJDIR}/event/lib
LIBS += -levent -lcrypto -lmtp -ltb
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)
install: all
-mkdir -p $(INSTALL_DIR)/opsdir/sbin
$(INSTALL_PROGRAM) emcd $(INSTALL_DIR)/opsdir/sbin/emcd
clean:
rm -f *.o
This diff is collapsed.
#ifndef __EMCD_H__
#define __EMCD_H__
struct rmc_client {
int sock_fd;
struct robot_list *position_list;
};
struct vmc_client {
int sock_fd;
struct robot_list *position_list;
};
#define EMC_SERVER_PORT 2525
#endif
#include "robot_list.h"
#include "config.h"
#include <stdlib.h>
#include "robot_list.h"
struct robot_list *robot_list_create() {
struct robot_list *tmp = (struct robot_list *)malloc(sizeof(struct robot_list));
......@@ -202,6 +206,9 @@ struct robot_list_enum *robot_list_enum(struct robot_list *l) {
return NULL;
}
else {
struct robot_list_item *a;
int i;
tmp = (void *)malloc(sizeof(void *)*(l->item_count));
if (tmp == NULL) {
free(e);
......@@ -213,9 +220,9 @@ struct robot_list_enum *robot_list_enum(struct robot_list *l) {
e->size = l->item_count;
/* scroll through list forwards and copy data into e->data */
struct robot_list_item *a = l->head;
int i = 0;
a = l->head;
i = 0;
while (a != NULL) {
e->data[i] = a->data;
a = a->next;
......
1 garcia1.flux.utah.edu 5 6 0.38
2 garcia2.flux.utah.edu 7 8 0.46
#! /bin/sh
## Variables
# The full path of the test case
test_file=$1
# The base name of the test case
test_file_base="test_emcd.sh"
# The current test number for shell based tests.
test_num=0
SRCDIR=@srcdir@
PORT=6060
## 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`
}
##
emcd -l `pwd`/test_emcd.log \
-i `pwd`/test_emcd.pid \
-p ${PORT} \
-s ops \
-c `realpath ${SRCDIR}/test_emcd.config`
trap 'kill `cat test_emcd.pid`' EXIT
sleep 2
run_test ../mtp/mtp_send -n localhost -P ${PORT} \
-w -r rmc -i 0 -c 0 -m "empty" init -- \
-w -r rmc -i 1 request-position
check_output "init -> request-position failed?" <<EOF
Packet: length 71; version 1; role 0
opcode: config-rmc
num: 2
horiz: 3.657600
vert: 2.438400
robot[0]: 1, garcia1.flux.utah.edu
robot[1]: 2, garcia2.flux.utah.edu
Packet: length 40; version 1; role 0
opcode: error
id: -1
code: -1
msg: position not updated yet
EOF
run_test ../mtp/mtp_send -n localhost -P ${PORT} \
-w -r rmc -i 0 -c 0 -m "empty" init -- \
-r rmc -i 1 -x 6 -y 7 -o 0.44 -s idle -t 1 update-position -- \
-w -r rmc -i 1 request-position
check_output "init -> update-position -> request-position failed?" <<EOF
Packet: length 71; version 1; role 0
opcode: config-rmc
num: 2
horiz: 3.657600
vert: 2.438400
robot[0]: 1, garcia1.flux.utah.edu
robot[1]: 2, garcia2.flux.utah.edu
Packet: length 31; version 1; role 0
opcode: update-position
id: 1
x: 6.000000
y: 7.000000
theta: 0.440000
status: -1
timestamp: 1
EOF
......@@ -41,7 +41,10 @@ mtp_send: mtp_send.o $(MTPLIBS)
mtp_recv: mtp_recv.o $(MTPLIBS)
$(CC) $(CFLAGS) $(LDFLAGS) -o $@ mtp_recv.o -L. -lmtp
install: $(addprefix $(INSTALL_LIBDIR), $(MTPLIBS))
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
clean:
/bin/rm -f *.o *.a $(MTPPROGS)
......@@ -83,6 +83,28 @@ int mtp_receive_packet(int fd,struct mtp_packet **packet) {
}
#define encode_float32(buf, idx, val) { \
union { \
float f; \
unsigned int i; \
} __z; \
\
__z.f = val; \
*((int *)((buf)+(idx))) = htonl(__z.i); \
idx += 4; \
}
#define decode_float32(buf, idx, val) { \
union { \
float f; \
unsigned int i; \
} __z; \
\
__z.i = ntohl(*((int *)((buf)+(idx)))); \
val = __z.f; \
idx += 4; \
}
int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
char *buf;
int i,j;
......@@ -156,10 +178,8 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
i += strlen(&buf[i]) + 1;
}
// now write the global_bound data
*((int *)(buf+i)) = htonl(data->box.horizontal);
i += 4;
*((int *)(buf+i)) = htonl(data->box.vertical);
encode_float32(buf, i, data->box.horizontal);
encode_float32(buf, i, data->box.vertical);
}
else if (packet->opcode == MTP_CONFIG_VMC) {
struct mtp_config_rmc *data = packet->data.config_rmc;
......@@ -229,12 +249,9 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
// now write the specific data:
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->position.x);
i += 4;
*((int *)(buf+i)) = htonl(data->position.y);
i += 4;
*((int *)(buf+i)) = htonl(data->position.theta);
i += 4;
encode_float32(buf, i, data->position.x);
encode_float32(buf, i, data->position.y);
encode_float32(buf, i, data->position.theta);
*((int *)(buf+i)) = htonl(data->timestamp);
i += 4;
......@@ -258,12 +275,9 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
i = MTP_PACKET_HEADER_LEN;
*((int *)(buf+i)) = htonl(data->robot_id);
i += 4;
*((int *)(buf+i)) = htonl(data->position.x);
i += 4;
*((int *)(buf+i)) = htonl(data->position.y);
i += 4;
*((int *)(buf+i)) = htonl(data->position.theta);
i += 4;
encode_float32(buf, i, data->position.x);
encode_float32(buf, i, data->position.y);
encode_float32(buf, i, data->position.theta);
*((int *)(buf+i)) = htonl(data->status);
i += 4;
*((int *)(buf+i)) = htonl(data->timestamp);
......@@ -312,12 +326,9 @@ int mtp_encode_packet(char **buf_ptr,struct mtp_packet *packet) {
i += 4;
*((int *)(buf+i)) = htonl(data->robot_id);
i += 4;
*((int *)(buf+i)) = htonl(data->position.x);
i += 4;
*((int *)(buf+i)) = htonl(data->position.y);
i += 4;
*((int *)(buf+i)) = htonl(data->position.theta);
i += 4;
encode_float32(buf, i, data->position.x);
encode_float32(buf, i, data->position.y);
encode_float32(buf, i, data->position.theta);
}
else if (packet->opcode == MTP_COMMAND_STOP) {
......@@ -417,11 +428,8 @@ int mtp_decode_packet(char *buf,struct mtp_packet **packet_ptr) {
i += len;
}
// write the global_coord box
data->box.horizontal = ntohl(*((int *)(buf+i)));
i += 4;
data->box.vertical = ntohl(*((int *)(buf+i)));
i += 4;
decode_float32(buf, i, data->box.horizontal);
decode_float32(buf, i, data->box.vertical);
}
else if (packet->opcode == MTP_CONFIG_VMC) {
struct mtp_config_vmc *data = (struct mtp_config_vmc *)malloc(sizeof(struct mtp_config_vmc));
......@@ -463,12 +471,9 @@ int mtp_decode_packet(char *buf,struct mtp_packet **packet_ptr) {
return MTP_PP_ERROR_MALLOC;
}
packet->data.request_id = data;
data->position.x = ntohl(*((int *)(buf+i)));
i += 4;
data->position.y = ntohl(*((int *)(buf+i)));
i += 4;
data->position.theta = ntohl(*((int *)(buf+i)));
i += 4;
decode_float32(buf, i, data->position.x);
decode_float32(buf, i, data->position.y);
decode_float32(buf, i, data->position.theta);
data->timestamp = ntohl(*((int *)(buf+i)));
i += 4;
......@@ -481,12 +486,9 @@ int mtp_decode_packet(char *buf,struct mtp_packet **packet_ptr) {
packet->data.update_position = data;
data->robot_id = ntohl(*((int *)(buf+i)));
i += 4;
data->position.x = ntohl(*((int *)(buf+i)));
i += 4;
data->position.y = ntohl(*((int *)(buf+i)));
i += 4;
data->position.theta = ntohl(*((int *)(buf+i)));
i += 4;
decode_float32(buf, i, data->position.x);
decode_float32(buf, i, data->position.y);
decode_float32(buf, i, data->position.theta);
data->status = ntohl(*((int *)(buf+i)));
i += 4;
data->timestamp = ntohl(*((int *)(buf+i)));
......@@ -513,12 +515,9 @@ int mtp_decode_packet(char *buf,struct mtp_packet **packet_ptr) {
i += 4;
data->robot_id = ntohl(*((int *)(buf+i)));
i += 4;
data->position.x = ntohl(*((int *)(buf+i)));
i += 4;
data->position.y = ntohl(*((int *)(buf+i)));
i += 4;
data->position.theta = ntohl(*((int *)(buf+i)));
i += 4;
decode_float32(buf, i, data->position.x);
decode_float32(buf, i, data->position.y);
decode_float32(buf, i, data->position.theta);
}
else if (packet->opcode == MTP_COMMAND_STOP) {
......@@ -680,6 +679,7 @@ int mtp_calc_size(int opcode,void *data) {
opcode == MTP_CONTROL_INIT ||
opcode == MTP_CONTROL_CLOSE) {
struct mtp_control *c = (struct mtp_control *)data;
assert(c->msg);
retval += 8;
retval += strlen(c->msg) + 1;
}
......@@ -687,6 +687,8 @@ int mtp_calc_size(int opcode,void *data) {
struct mtp_config_rmc *c = (struct mtp_config_rmc *)data;
retval += 4;
for (i = 0; i < c->num_robots; ++i) {
assert(c->robots[i].hostname);
retval += 4;
retval += strlen(c->robots[i].hostname) + 1;
}
......@@ -696,6 +698,8 @@ int mtp_calc_size(int opcode,void *data) {
struct mtp_config_vmc *c = (struct mtp_config_vmc *)data;
retval += 4;
for (i = 0; i < c->num_robots; ++i) {
assert(c->robots[i].hostname);
retval += 4;
retval += strlen(c->robots[i].hostname) + 1;
}
......@@ -731,3 +735,151 @@ int mtp_calc_size(int opcode,void *data) {
return retval;
}
void mtp_print_packet(FILE *file, struct mtp_packet *mp)
{
int lpc;
fprintf(file,
"Packet: length %d; version %d; role %d\n",
mp->length,
mp->version,
mp->role);
switch (mp->opcode) {
case MTP_CONTROL_ERROR:
case MTP_CONTROL_NOTIFY:
case MTP_CONTROL_INIT:
case MTP_CONTROL_CLOSE:
switch (mp->opcode) {
case MTP_CONTROL_ERROR:
fprintf(file, " opcode:\terror\n");
break;
case MTP_CONTROL_NOTIFY:
fprintf(file, " opcode:\tnotify\n");
break;
case MTP_CONTROL_INIT:
fprintf(file, " opcode:\tinit\n");
break;
case MTP_CONTROL_CLOSE:
fprintf(file, " opcode:\tclose\n");
break;
}
fprintf(file,
" id:\t\t%d\n"
" code:\t\t%d\n"
" msg:\t\t%s\n",
mp->data.control->id,
mp->data.control->code,
mp->data.control->msg);
break;
case MTP_CONFIG_VMC:
fprintf(file,
" opcode:\tconfig-vmc\n"
" num:\t\t%d\n",
mp->data.config_vmc->num_robots);
for (lpc = 0;
lpc < mp->data.config_vmc->num_robots;
lpc++) {
fprintf(file,
" robot[%d]:\t%d, %s\n",
lpc,
mp->data.config_vmc->robots[lpc].id,
mp->data.config_vmc->robots[lpc].hostname);
}
break;
case MTP_CONFIG_RMC:
fprintf(file,
" opcode:\tconfig-rmc\n"
" num:\t\t%d\n"
" horiz:\t%f\n"
" vert:\t\t%f\n",
mp->data.config_rmc->num_robots,
mp->data.config_rmc->box.horizontal,
mp->data.config_rmc->box.vertical);
for (lpc = 0;
lpc < mp->data.config_rmc->num_robots;
lpc++) {
fprintf(file,
" robot[%d]:\t%d, %s\n",
lpc,
mp->data.config_rmc->robots[lpc].id,
mp->data.config_rmc->robots[lpc].hostname);
}
break;
case MTP_REQUEST_POSITION:
fprintf(file,
" opcode:\trequest-position\n"
" id:\t\t%d\n",
mp->data.request_position->robot_id);
break;
case MTP_REQUEST_ID:
fprintf(file,
" opcode:\trequest-id\n"
" x:\t\t%f\n"
" y:\t\t%f\n"
" theta:\t%f\n"
" timestamp:\t%d\n",
mp->data.request_id->position.x,
mp->data.request_id->position.y,
mp->data.request_id->position.theta,
mp->data.request_id->timestamp);
break;
case MTP_UPDATE_POSITION:
fprintf(file,
" opcode:\tupdate-position\n"
" id:\t\t%d\n"
" x:\t\t%f\n"
" y:\t\t%f\n"
" theta:\t%f\n"
" status:\t%d\n"
" timestamp:\t%d\n",
mp->data.update_position->robot_id,
mp->data.update_position->position.x,
mp->data.update_position->position.y,
mp->data.update_position->position.theta,
mp->data.update_position->status,
mp->data.update_position->timestamp);
break;
case MTP_UPDATE_ID:
fprintf(file,
" opcode:\tupdate-id\n"
" id:\t%d\n",
mp->data.update_position->robot_id);
break;
case MTP_COMMAND_GOTO:
fprintf(file,
" opcode:\tcommand-goto\n"
" commid:\t%d\n"
" id:\t\t%d\n"
" x:\t\t%f\n"
" y:\t\t%f\n"
" theta:\t%f\n",
mp->data.command_goto->command_id,
mp->data.command_goto->robot_id,
mp->data.command_goto->position.x,
mp->data.command_goto->position.y,
mp->data.command_goto->position.theta);
break;
case MTP_COMMAND_STOP:
fprintf(file,
" opcode:\tupdate-id\n"
" commid:\t%d\n"
" id:\t%d\n",
mp->data.command_stop->command_id,
mp->data.command_stop->robot_id);
break;
default:
assert(0);
break;
}
}
......@@ -185,6 +185,8 @@ struct mtp_packet *mtp_make_packet( unsigned char opcode,
/* data's type is only known by opcode */
void *data
);
void mtp_free_packet(struct mtp_packet *mp);
void mtp_print_packet(FILE *file, struct mtp_packet *mp);
/* data's type is known by opcode! */
int mtp_calc_size(int opcode,void *data);
......
......@@ -64,139 +64,8 @@ int main(int argc, char *argv[])
struct mtp_packet *mp;
while (mtp_receive_packet(fd_peer, &mp) == MTP_PP_SUCCESS) {
int lpc;
mtp_print_packet(stdout, mp);
printf("Packet: length %d; version %d; role %d\n",
mp->length,
mp->version,
mp->role);
switch (mp->opcode) {
case MTP_CONTROL_ERROR:
case MTP_CONTROL_NOTIFY:
case MTP_CONTROL_INIT:
case MTP_CONTROL_CLOSE:
switch (mp->opcode) {
case MTP_CONTROL_ERROR:
printf(" opcode:\terror\n");
break;
case MTP_CONTROL_NOTIFY:
printf(" opcode:\tnotify\n");
break;
case MTP_CONTROL_INIT:
printf(" opcode:\tinit\n");
break;
case MTP_CONTROL_CLOSE:
printf(" opcode:\tclose\n");
break;
}
printf(" id:\t\t%d\n"
" code:\t\t%d\n"
" msg:\t\t%s\n",
mp->data.control->id,
mp->data.control->code,
mp->data.control->msg);
break;
case MTP_CONFIG_VMC:
printf(" opcode:\tconfig-vmc\n"
" num:\t\t%d\n",
mp->data.config_vmc->num_robots);
for (lpc = 0;
lpc < mp->data.config_vmc->num_robots;
lpc++) {
printf(" robot[%d]:\t%d, %s\n",
lpc,
mp->data.config_vmc->robots[lpc].id,
mp->data.config_vmc->robots[lpc].hostname);
}
break;
case MTP_CONFIG_RMC:
printf(" opcode:\tconfig-rmc\n"
" num:\t%d\n"
" horiz:\t%f\n"
" vert:\t%f\n",
mp->data.config_rmc->num_robots,
mp->data.config_rmc->box.horizontal,
mp->data.config_rmc->box.vertical);
for (lpc = 0;
lpc < mp->data.config_rmc->num_robots;
lpc++) {
printf(" robot[%d]:\t%d, %s\n",
lpc,
mp->data.config_rmc->robots[lpc].id,
mp->data.config_rmc->robots[lpc].hostname);
}