Commit 57ddedbe authored by Timothy Stack's avatar Timothy Stack

More bug fixes for vmcd and friends:

	* GNUmakerules: Set SRCDIR as an environment variable so every
	test doesn't have to be a template.

	* robots/emc/emcd.c: Correctly handle a vision position that is
	dead-on the initial position.  XXX Turn off orientation matching
	for now, it is too strict at the moment so that it favors close
	orientation over close position.

	* robots/mtp/mtp.c: Fix some broken printfs in mtp_print_packet.

	* robots/vmcd/GNUmakefile.in: Add another test.

	* robots/vmcd/test_emcd2.config, robots/vmcd/test_vmcd2.pos,
	robots/vmcd/etst_vmcd2.sh: A test that matches the initial
	positions of two robots.

	* robots/vmcd/test_vmcd.pos: Reduce the distance the robot moved
	between frames.

	* robots/vmcd/vmcd.c: Fix a bugfix and some formatting changes.
parent b63374eb
......@@ -73,7 +73,7 @@ ifeq ($(TESTS),)
.PHONY: check
else
check: $(TESTS)
@for tst in $^; do \
@export SRCDIR=${SRCDIR}; for tst in $^; do \
if test `basename $$tst .sh` != `basename $$tst`; then \
tst="sh $$tst"; \
fi; \
......
......@@ -1248,19 +1248,27 @@ int vmc_callback(elvin_io_handler_t handler,
dx = rid->position.x - p->x;
dy = rid->position.y - p->y;
ddist = sqrt(dx*dx + dy*dy);
if (dx == 0 && dy == 0)
ddist = 0;
else
ddist = sqrt(dx*dx + dy*dy);
dtheta = rid->position.theta - p->theta;
info("dx %f %f %f %f\n", dx, dy, ddist, dtheta);
if (fabsf(ddist) > best_dist_delta) {
info("pass ? %f %f\n", ddist, best_dist_delta);
i = i->next;
continue;
}
if (fabsf(dtheta) > best_pose_delta) {
i = i->next;
continue;
}
if (0) {
if (fabsf(dtheta) > best_pose_delta) {
i = i->next;
continue;
}
}
info("match %d\n", id);
best_dist_delta = ddist;
best_pose_delta = dtheta;
robot_id = id;
......@@ -1272,6 +1280,8 @@ int vmc_callback(elvin_io_handler_t handler,
// now send whatever we got to the caller as we fall through:
}
info("match %f %f -> %d\n",
rid->position.x, rid->position.y, robot_id);
// we want to return whichever robot id immediately to the caller:
uid.request_id = rid->request_id;
......
......@@ -882,10 +882,10 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
case MTP_UPDATE_ID:
fprintf(file,
" opcode:\tupdate-id\n"
" request_id:\t%d\n"
" request_id:\t%d\n"
" id:\t%d\n",
mp->data.request_id->request_id,
mp->data.update_position->robot_id);
mp->data.update_id->request_id,
mp->data.update_id->robot_id);
break;
case MTP_COMMAND_GOTO:
......@@ -902,7 +902,7 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
mp->data.command_goto->position.x,
mp->data.command_goto->position.y,
mp->data.command_goto->position.theta,
mp->data.update_position->position.timestamp);
mp->data.command_goto->position.timestamp);
break;
case MTP_COMMAND_STOP:
......
......@@ -13,7 +13,7 @@ include $(OBJDIR)/Makeconf
PROGS = vmc-client vmcd
TESTS = test_vmc-client.sh
TESTS += test_vmcd.sh
TESTS += test_vmcd.sh test_vmcd2.sh
all: $(PROGS)
......
1 garcia1.flux.utah.edu 5 6 0.38 node1
2 garcia2.flux.utah.edu 20 20 0.0 node2
1
5 6 0.38
1
5.1 6 0.38
5.04 6 0.38
......@@ -9,7 +9,7 @@ test_file_base="test_vmcd.sh"
# The current test number for shell based tests.
test_num=0
SRCDIR=@srcdir@
# SRCDIR=@srcdir@
EMC_PORT=6565
VMC1_PORT=6566
......@@ -81,7 +81,7 @@ check_output "no update?" <<EOF
Packet: length 36; version 1; role 0
opcode: update-position
id: 1
x: 5.100000
x: 5.040000
y: 6.000000
theta: 0.380000
status: -1
......
2
5 6 0.38
20 20 0.0
2
5.04 6 0.38
20 20 0.0
2
5.04 6 0.38
20 20 0.0
2
5.04 6 0.38
20 20 0.0
#! /bin/sh
## Variables
# The full path of the test case
test_file=$1
# The base name of the test case
test_file_base="test_vmcd2.sh"
# The current test number for shell based tests.
test_num=0
# SRCDIR=@srcdir@
EMC_PORT=6565
VMC1_PORT=6566
## 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`
}
##
# Start the daemons vmcd depends on:
../emc/emcd -l `pwd`/test_emcd.log \
-i `pwd`/test_emcd.pid \
-p ${EMC_PORT} \
-s ops \
-c `realpath ${SRCDIR}/test_emcd2.config`
vmc-client -l `pwd`/test_vmc-client1.log \
-i `pwd`/test_vmc-client1.pid \
-p ${VMC1_PORT} \
-f ${SRCDIR}/test_vmcd2.pos \
foobar
# Start vmcd:
vmcd -l `pwd`/test_vmcd.log \
-i `pwd`/test_vmcd.pid \
-e localhost \
-p ${EMC_PORT} \
-c localhost -P ${VMC1_PORT}
cleanup() {
kill `cat test_vmcd.pid`
kill `cat test_emcd.pid`
kill `cat test_vmc-client1.pid`
}
trap 'cleanup' EXIT
sleep 2
newframe() {
kill -s USR1 `cat test_vmc-client1.pid`
}
newframe
sleep 1
newframe
sleep 1
run_test ../mtp/mtp_send -n localhost -P ${EMC_PORT} \
-r emulab -i 1 -c 0 -m "empty" init -- \
-w -i 1 request-position
check_output "no update?" <<EOF
Packet: length 36; version 1; role 0
opcode: update-position
id: 1
x: 5.040000
y: 6.000000
theta: 0.380000
status: -1
timestamp: 20.000000
EOF
run_test ../mtp/mtp_send -n localhost -P ${EMC_PORT} \
-r emulab -i 1 -c 0 -m "empty" init -- \
-w -i 2 request-position
check_output "no update?" <<EOF
Packet: length 36; version 1; role 0
opcode: update-position
id: 2
x: 20.000000
y: 20.000000
theta: 0.000000
status: -1
timestamp: 20.000000
EOF
......@@ -505,14 +505,16 @@ void vmc_handle_update_position(struct vmc_client *vc,struct mtp_packet *p) {
float my_dist_delta;
float my_pose_delta;
dx = p->data.update_position->position.x - vc->tracks[i].position.x;
dy = p->data.update_position->position.y - vc->tracks[i].position.y;
dx = fabsf(p->data.update_position->position.x -
vc->tracks[i].position.x);
dy = fabsf(p->data.update_position->position.y -
vc->tracks[i].position.y);
my_dist_delta = sqrt(dx*dx + dy*dy);
my_pose_delta = p->data.update_position->position.theta -
vc->tracks[i].position.theta;
if (fabsf(my_dist_delta) < DIST_THRESHOLD ||
fabsf(my_dist_delta) < best_dist_delta) {
if (fabsf(my_dist_delta) > DIST_THRESHOLD ||
fabsf(my_dist_delta) > best_dist_delta) {
continue;
}
if (fabsf(my_pose_delta) > POSE_THRESHOLD ||
......@@ -534,15 +536,18 @@ void vmc_handle_update_position(struct vmc_client *vc,struct mtp_packet *p) {
// now check to see if we found a match.
// if we did, update the track:
if (best_track_idx > -1) {
info("got a match\n");
vc->tracks[best_track_idx].position = p->data.update_position->position;
info("got a match\n");
vc->tracks[best_track_idx].position =
p->data.update_position->position;
// now we try to update real_robots
if (vc->tracks[best_track_idx].robot_id != -1) {
// we want to update the real_robots list!
for (j = 0; j < MAX_TRACKED_OBJECTS; ++j) {
if (real_robots[j].robot_id ==
vc->tracks[best_track_idx].robot_id) {
real_robots[j].position = vc->tracks[best_track_idx].position;
if (real_robots[j].robot_id ==
vc->tracks[best_track_idx].robot_id) {
real_robots[j].position =
vc->tracks[best_track_idx].position;
}
}
}
......@@ -556,10 +561,11 @@ void vmc_handle_update_position(struct vmc_client *vc,struct mtp_packet *p) {
printf("OUT OF TRACKING ROOM IN VMCD!\n");
}
else {
int retval;
int retval;
vc->tracks[first_invalid_track].valid = 1;
vc->tracks[first_invalid_track].position = p->data.update_position->position;
vc->tracks[first_invalid_track].position =
p->data.update_position->position;
vc->tracks[first_invalid_track].robot_id = -1;
vc->tracks[first_invalid_track].request_id = -1;
......@@ -577,8 +583,9 @@ void vmc_handle_update_position(struct vmc_client *vc,struct mtp_packet *p) {
mp = mtp_make_packet(MTP_REQUEST_ID, MTP_ROLE_VMC, &rid);
retval = mtp_send_packet(emc_fd,mp);
if (retval == MTP_PP_SUCCESS) {
info("sent request %d\n", rid.request_id);
vc->tracks[first_invalid_track].request_id = rid.request_id;
info("sent request %d\n", rid.request_id);
vc->tracks[first_invalid_track].request_id =
rid.request_id;
}
else {
vc->tracks[first_invalid_track].valid = 0;
......@@ -586,8 +593,10 @@ void vmc_handle_update_position(struct vmc_client *vc,struct mtp_packet *p) {
}
else {
// we can update the real_robots list!
real_robots[retval].position = p->data.update_position->position;
vc->tracks[first_invalid_track].robot_id = real_robots[retval].robot_id;
real_robots[retval].position =
p->data.update_position->position;
vc->tracks[first_invalid_track].robot_id =
real_robots[retval].robot_id;
}
vc->tracks[first_invalid_track].updated = 1;
......@@ -608,7 +617,11 @@ void vmc_handle_update_position(struct vmc_client *vc,struct mtp_packet *p) {
vc->tracks[i].valid = -1;
}
else if (vc->tracks[i].valid && vc->tracks[i].robot_id != -1) {
info("sending update for %d\n",vc->tracks[i].robot_id);
info("sending update for %d -> %f %f\n",
vc->tracks[i].robot_id,
vc->tracks[i].position.x,
vc->tracks[i].position.y);
mup.robot_id = vc->tracks[i].robot_id;
mup.position = vc->tracks[i].position;
mup.status = MTP_POSITION_STATUS_UNKNOWN;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment