Commit dde7799c authored by Timothy Stack's avatar Timothy Stack

More robot-related tweaks and bug fixes:

	* robots/emc/emcd.c: Handle "-h" option properly.

	* robots/emc/loclistener.in: Pickup the pixels_per_meter value
	from the database and use that to convert event coordinates in
	meters to pixels for the database.

	* robots/primotion/gorobot.cc: Comment.

	* robots/primotion/dgrobot/gcallbacks.h,
	robots/primotion/dgrobot/gcallbacks.cc,
	robots/primotion/dgrobot/grobot.h,
	robots/primotion/dgrobot/grobot.c,
	robots/primotion/dgrobot/grobot_sim.cc: Small bug fixes to make
	sure the position estimate is updated when moving in a straight
	line.

	* robots/rmcd/rmcd.c: cleanup

	* robots/vmcd/GNUmakefile.in, robots/vmcd/test_emcd3.config,
	robots/vmcd/test_vmcd3.pos, robots/vmcd/test_vmcd3.sh,
	robots/vmcd/test_vmcd4.pos, robots/vmcd/test_vmcd4.sh: Some more
	test cases, still pretty simple though.
parent 0132fe12
......@@ -167,7 +167,16 @@ static void sigpanic(int sig)
static void usage(char *progname)
{
fprintf(stderr,
"Usage: emcd\n");
"Usage: emcd [-hd] [-c config] [-e pid/eid] [-k keyfile] [...]\n"
"Options:\n"
" -h\t\tPrint this message.\n"
" -d\t\tTurn on debugging messages and do not daemonize\n"
" -e pid/eid\tConnect to the experiment event server\n"
" -k keyfile\tKeyfile for the experiment\n"
" -p port\tPort to listen on for mtp messages\n"
" -c config\tThe config file to use\n"
" -l logfile\tThe log file to use\n"
" -i pidfile\tThe file to write the PID to\n");
}
int main(int argc, char *argv[])
......@@ -193,6 +202,8 @@ int main(int argc, char *argv[])
while ((c = getopt(argc, argv, "hde:k:p:c:f:s:P:l:i:")) != -1) {
switch (c) {
case 'h':
usage(progname);
exit(0);
break;
case 'd':
debug += 1;
......
......@@ -56,6 +56,18 @@ except getopt.error:
sys.exit(2)
pass
res = DBQueryFatal("SELECT pixels_per_meter FROM floorimages "
"WHERE building='MEB-ROBOTS'");
if len(res) == 0:
sys.stderr.write("error: MEB-ROBOTS building does not exist?\n");
sys.exit(2)
pass
pixels_per_meter = float(res[0][0])
sys.stderr.write("Pixels-per-meter: %f\n" % pixels_per_meter)
class LocationInfoUpdater(EventClient):
def handle_event(self, event):
......@@ -87,10 +99,13 @@ class LocationInfoUpdater(EventClient):
try:
key, value = arg.split('=')
if key == "X" or key == "x":
update["loc_x"] = str(float(value))
update["loc_x"] = str(
float(value) * pixels_per_meter)
pass
elif key == "Y" or key == "y":
update["loc_y"] = str(float(value))
# XXX 520 is hardcoded
update["loc_y"] = str(
520 - float(value) * pixels_per_meter)
pass
else:
sys.stderr.write("warning: unknown key: %s\n" %
......
......@@ -9,7 +9,7 @@
#include "gcallbacks.h"
CallbackComplete::CallbackComplete(acpObject *b, grobot *g) {
CallbackComplete::CallbackComplete(acpObject *b, grobot *g, cb_type_t cbt) {
behavior = b;
pgrobot = g;
......@@ -17,6 +17,8 @@ CallbackComplete::CallbackComplete(acpObject *b, grobot *g) {
// get ID of current behavior
blast_id = behavior->getNamedValue("unique-id")->getIntVal();
this->cbt = cbt;
//strcpy(lastMSG, "no messages");
// cout << "Created Callback" << endl;
......@@ -40,7 +42,7 @@ aErr CallbackComplete::call() {
// call completion callback
blast_status = behavior->getNamedValue("completion-status")->getIntVal();
pgrobot->setCBstatus(blast_id, blast_status);
pgrobot->setCBstatus(blast_id, blast_status, this->cbt);
}
......
......@@ -23,7 +23,7 @@ class CallbackExecute;
// a simple callback completion class
class CallbackComplete : public acpCallback {
public:
CallbackComplete(acpObject *b, grobot *g);
CallbackComplete(acpObject *b, grobot *g, cb_type_t cbt);
~CallbackComplete();
aErr call();
......@@ -33,7 +33,8 @@ class CallbackComplete : public acpCallback {
private:
acpObject *behavior;
grobot *pgrobot;
cb_type_t cbt;
int blast_status; // Behavior Last _STATUS
int blast_id; // Behavior Last _ID
};
......
......@@ -7,6 +7,7 @@
*/
#include "grobot.h"
#include "gcallbacks.h"
grobot::grobot() {
......@@ -131,7 +132,7 @@ void grobot::pbMove(float mdisplacement) {
pBehavior = garcia.createNamedBehavior("move", "move1");
pBehavior->setNamedValue("distance", &moveLength);
createPRIMbehavior();
createPRIMbehavior(CBT_MOVE);
}
......@@ -144,7 +145,7 @@ void grobot::pbPivot(float pangle) {
pBehavior = garcia.createNamedBehavior("pivot", "pivot1");
pBehavior->setNamedValue("angle", &pivotAngle);
createPRIMbehavior();
createPRIMbehavior(CBT_PIVOT);
}
......@@ -205,8 +206,10 @@ void grobot::dgoto(float Dx, float Dy, float Rf) {
++gotomexec;
++gotomcomplete;
gotop2 = 0;
set_gotocomplete();
}
if (0.0f == dt_init && 0.0f == moveL && 0.0f == Rfr)
set_gotocomplete();
} else {
// if a goto is already executing, drop the command
......@@ -280,6 +283,7 @@ int grobot::getGOTOstatus() {
if (0 != gotocomplete) {
// reset the status
gotolock = 0;
gotocomplete = 0;
}
return retval_gotocomplete;
......@@ -331,7 +335,7 @@ void grobot::setCBexec(int id) {
void grobot::setCBstatus(int id, int stat) {
void grobot::setCBstatus(int id, int stat, cb_type_t cbt) {
// set completion callback status
cout << "Behavior #" << id
......@@ -341,6 +345,14 @@ void grobot::setCBstatus(int id, int stat) {
if (0 != gotolock) {
// a goto behavior is currently executing
++gotomcomplete;
if (cbt == CBT_MOVE) {
// get the Arclength, then estimate and store the displacement
// (assume that the first pivot was accurate)
float al_temp = getArclen();
dx_est = al_temp * cos(dt_init);
dy_est = al_temp * sin(dt_init);
}
if (1 == gotomcomplete) {
// first pivot has finished
......@@ -349,12 +361,6 @@ void grobot::setCBstatus(int id, int stat) {
// main move has executed
gotom1 = stat;
// get the Arclength, then estimate and store the displacement
// (assume that the first pivot was accurate)
float al_temp = getArclen();
dx_est = al_temp * cos(dt_init);
dy_est = al_temp * sin(dt_init);
} else if (3 == gotomcomplete) {
// second pivot has executed
gotop2 = stat;
......@@ -392,7 +398,7 @@ void grobot::createNULLbehavior() {
acpValue acceleration(2.0f);
pBehavior = garcia.createNamedBehavior("null", "driver");
completeCB = new CallbackComplete(pBehavior, this);
completeCB = new CallbackComplete(pBehavior, this, CBT_NONE);
completeCBacpV.set(completeCB);
pBehavior->setNamedValue("completion-callback", &completeCBacpV);
......@@ -404,7 +410,7 @@ void grobot::createNULLbehavior() {
void grobot::createPRIMbehavior() {
void grobot::createPRIMbehavior(cb_type_t cbt) {
// create and execute a PRIMitive behavior
// wait until garcia is ready
......@@ -421,7 +427,7 @@ void grobot::createPRIMbehavior() {
}
executeCB = new CallbackExecute(pBehavior, this);
completeCB = new CallbackComplete(pBehavior, this);
completeCB = new CallbackComplete(pBehavior, this, cbt);
executeCBacpV.set(executeCB);
......
......@@ -11,13 +11,12 @@
#define GROBOT_H
class grobot;
class CallbackComplete;
class CallbackExecute;
#if !defined(GROBOT_SIM)
#include "acpGarcia.h"
#include "acpValue.h"
#include "gcallbacks.h"
#include "gbehaviors.h"
#endif
#include <math.h>
......@@ -29,6 +28,13 @@ class grobot;
// track width, in meters
#define TRACK_WIDTH 0.1778f
typedef enum {
CBT_NONE,
CBT_PIVOT,
CBT_MOVE
} cb_type_t;
class grobot {
public:
grobot();
......@@ -52,7 +58,7 @@ class grobot {
void sleepy();
void setCBexec(int id);
void setCBstatus(int id, int stat);
void setCBstatus(int id, int stat, cb_type_t cbt);
#if !defined(GROBOT_SIM)
acpGarcia garcia;
......@@ -60,7 +66,7 @@ class grobot {
private:
void createNULLbehavior();
void createPRIMbehavior();
void createPRIMbehavior(cb_type_t cbt);
void set_gotocomplete();
......
......@@ -91,7 +91,7 @@ void grobot::setCBexec(int id)
{
}
void grobot::setCBstatus(int id, int status)
void grobot::setCBstatus(int id, int status, cb_type_t cbt)
{
}
......@@ -99,7 +99,7 @@ void grobot::createNULLbehavior()
{
}
void grobot::createPRIMbehavior()
void grobot::createPRIMbehavior(cb_type_t cbt)
{
}
......
......@@ -105,7 +105,7 @@ int main(int argc, char *argv[])
}
}
printf("Listenting on %d\n", port);
printf("Listening on %d\n", port);
signal(SIGQUIT, sigquit);
signal(SIGTERM, sigquit);
......@@ -150,6 +150,7 @@ int main(int argc, char *argv[])
fd_set rreadyfds = readfds;
int rc;
/* Poll the file descriptors, don't block */
rc = select(FD_SETSIZE, &rreadyfds, NULL, NULL, &tv_zero);
if (rc > 0) {
int lpc;
......
......@@ -82,6 +82,7 @@ static struct mtp_config_rmc *rmc_config = NULL;
enum {
GCB_PENDING_POSITION,
GCB_REFINING_POSITION,
GCB_VISION_POSITION,
};
enum {
......@@ -94,6 +95,12 @@ enum {
* Flag used to indicate when we're trying to refine the position.
*/
GCF_REFINING_POSITION = (1L << GCB_REFINING_POSITION),
/**
* Flag used to indicate that the value in the gc_actual_pos field was
* obtained from the vision system.
*/
GCF_VISION_POSITION = (1L << GCB_VISION_POSITION),
};
struct gorobot_conn {
......@@ -179,14 +186,37 @@ static struct gorobot_conn *find_gorobot(int robot_id)
* Convert an absolute position to a relative position fit for grboto.dgoto().
*
* @param rel The object to fill out with the relative position.
* @param abs The current absolute position of the robot.
* @param abs_start The current absolute position of the robot.
* @param abs_finish The destination position for the robot.
*/
static void conv_a2r(struct position *rel,
struct position *abs_start,
struct position *abs_finish)
{
assert(rel != NULL);
assert(abs_start != NULL);
assert(abs_finish != NULL);
*rel = *abs_finish; // XXX DAN, fill this out.
}
/**
* Convert an relative movement to an absolute position that we can send back
* to emcd.
*
* @param abs_finish The object to fill out with the final absolute position.
* @param abs_start The absolute position of the robot before the move.
* @param rel The relative movement of the robot.
*/
static void conv_pos(struct position *rel, struct position *abs)
static void conv_r2a(struct position *abs_finish,
struct position *abs_start,
struct position *rel)
{
assert(rel != NULL);
assert(abs != NULL);
assert(abs_start != NULL);
assert(abs_finish != NULL);
*rel = *abs; // XXX DAN, fill this out.
*abs_finish = *rel; // XXX DAN, fill this out.
}
/**
......@@ -327,9 +357,13 @@ static void handle_emc_packet(int emc_fd)
error("uknnown robot %d\n",mp->data.update_position->robot_id);
}
else {
/* XXX Always update the position? */
/*
* XXX Always update the position? Should compare against the
* robot-derived position before overwriting it...
*/
gc->gc_actual_pos =
mp->data.update_position->position;
gc->gc_flags |= GCF_VISION_POSITION;
info("position update %f\n", gc->gc_actual_pos.x);
......@@ -401,11 +435,13 @@ static void handle_emc_packet(int emc_fd)
info("moving again\n");
/* Start the process all over again. */
/* Not there yet, contiue refining. */
mcg.command_id = 1;
mcg.robot_id = gc->gc_robot->id;
conv_pos(&mcg.position, &gc->gc_intended_pos);
conv_a2r(&mcg.position,
&gc->gc_actual_pos,
&gc->gc_intended_pos);
if ((gmp = mtp_make_packet(MTP_COMMAND_GOTO,
MTP_ROLE_RMC,
&mcg)) == NULL) {
......@@ -443,6 +479,30 @@ static void handle_gc_packet(struct gorobot_conn *gc, int emc_fd)
fatal("bad packet from gorobot!\n");
}
else if (mp->opcode == MTP_UPDATE_POSITION) {
struct mtp_update_position mup;
struct mtp_packet *ump;
/* Always update emcd and save the new estimated position. */
mup.robot_id = gc->gc_robot->id;
conv_r2a(&mup.position,
&gc->gc_actual_pos,
&mp->data.update_position->position);
gc->gc_actual_pos = mup.position;
gc->gc_flags &= ~GCF_VISION_POSITION;
mup.status = MTP_POSITION_STATUS_MOVING;
if ((ump = mtp_make_packet(MTP_UPDATE_POSITION,
MTP_ROLE_RMC,
&mup)) == NULL) {
fatal("cannot allocate packet");
}
else if (mtp_send_packet(emc_fd, ump) != MTP_PP_SUCCESS) {
fatal("request id failed");
}
mtp_free_packet(ump);
ump = NULL;
switch (mp->data.update_position->status) {
case MTP_POSITION_STATUS_IDLE:
/* Response to a COMMAND_STOP. */
......@@ -476,6 +536,31 @@ static void handle_gc_packet(struct gorobot_conn *gc, int emc_fd)
break;
case MTP_POSITION_STATUS_ERROR:
/* XXX */
{
struct mtp_update_position mup;
struct mtp_packet *ump;
mup.robot_id = gc->gc_robot->id;
mup.position = gc->gc_actual_pos;
mup.status = MTP_POSITION_STATUS_ERROR;
info("error for %d\n", mup.robot_id);
if ((ump = mtp_make_packet(MTP_UPDATE_POSITION,
MTP_ROLE_RMC,
&mup)) == NULL) {
fatal("cannot allocate packet");
}
else if (mtp_send_packet(emc_fd, ump) != MTP_PP_SUCCESS) {
fatal("request id failed");
}
mtp_free_packet(ump);
ump = NULL;
gc->gc_flags &= ~GCF_PENDING_POSITION;
gc->gc_flags &= ~GCF_REFINING_POSITION;
}
break;
case MTP_POSITION_STATUS_COMPLETE:
info("gorobot finished\n");
......@@ -662,6 +747,7 @@ int main(int argc, char *argv[])
}
else {
gc->gc_actual_pos = qrmp->data.update_position->position;
gc->gc_flags |= GCF_VISION_POSITION;
info(" robot[%d].x = %f\n", lpc, gc->gc_actual_pos.x);
info(" robot[%d].y = %f\n", lpc, gc->gc_actual_pos.y);
......
......@@ -13,7 +13,7 @@ include $(OBJDIR)/Makeconf
PROGS = vmc-client vmcd
TESTS = test_vmc-client.sh
TESTS += test_vmcd.sh test_vmcd2.sh
TESTS += test_vmcd.sh test_vmcd2.sh test_vmcd3.sh test_vmcd4.sh
all: $(PROGS)
......
1 garcia1.flux.utah.edu 5 6 0.0 node1
1
5.00 6 0.0
1
5.02 6 0.0
1
5.04 6 0.0
1
5.06 6 0.0
1
5.08 6 0.0
1
5.10 6 0.0
1
5.12 6 0.0
1
5.14 6 0.0
1
5.16 6 0.0
1
5.17 6 0.0
1
5.20 6 0.0
1
5.22 6 0.0
1
5.24 6 0.0
1
5.26 6 0.0
1
5.28 6 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_vmcd3.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_emcd3.config`
vmc-client -l `pwd`/test_vmc-client1.log \
-i `pwd`/test_vmc-client1.pid \
-p ${VMC1_PORT} \
-f ${SRCDIR}/test_vmcd3.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
lpc=0
while test $lpc -lt 14; do
newframe
sleep 0.1
lpc=`expr $lpc + 1`
done
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.280000
y: 6.000000
theta: 0.000000
status: -1
timestamp: 20.000000
EOF
2
5.00 6 0.38
20.00 20.00 0.0
2
5.04 6 0.38
20.02 20.00 0.0
2
5.06 6 0.38
20.04 20.00 0.0
2
5.08 6 0.38
20.06 20.00 0.0
2
5.10 6 0.38
20.08 20.00 0.0
2
5.12 6 0.38
20.10 20.00 0.0
2
5.14 6 0.38
20.12 20.00 0.0
2
5.16 6 0.38
20.14 20.00 0.0
2
5.18 6 0.38
20.16 20.00 0.0
2
5.20 6 0.38
20.18 20.00 0.0
2
5.22 6 0.38
20.20 20.00 0.0
2
5.24 6 0.38
20.22 20.00 0.0
2
5.26 6 0.38
20.24 20.00 0.0
2
5.28 6 0.38
20.26 20.00 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_vmcd4.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
}