Commit f81b860c authored by David Johnson's avatar David Johnson
Browse files

This is the big wiggle commit. All the necessary code is there, but needs

more testing.
parent 97f36a4e
......@@ -1079,6 +1079,25 @@ int rmc_callback(elvin_io_handler_t handler,
retval = 1;
}
break;
case MTP_WIGGLE_STATUS:
{
/* simply forward this to vmc */
/* actually, we'll also ack it back to rmc */
mp->role = MTP_ROLE_EMC;
if (mtp_send_packet(vmc_data.handle,mp) != MTP_PP_SUCCESS) {
error("vmc unavailable; cannot forward wiggle-status\n");
}
if (mtp_send_packet(rmc_data.handle,mp) != MTP_PP_SUCCESS) {
error("could not ack the wiggle-status packet from rmc;"
" this is a serious problem!\n"
);
}
retval = 1;
}
break;
default:
{
......@@ -1419,6 +1438,21 @@ int vmc_callback(elvin_io_handler_t handler,
retval = 1;
}
break;
case MTP_WIGGLE_REQUEST:
{
/* simply forward this to rmc */
if (rmc_data.handle != NULL) {
mp->role = MTP_ROLE_EMC;
mtp_send_packet(rmc_data.handle,mp);
}
else {
error("rmc unavailable; cannot forward wiggle-request\n");
}
retval = 1;
}
break;
default:
{
......
......@@ -390,6 +390,14 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.command_stop.robot_id =
va_arg(args, int);
break;
case MTP_WIGGLE_REQUEST:
mp->data.mtp_payload_u.wiggle_request.robot_id =
va_arg(args, int);
break;
case MTP_WIGGLE_STATUS:
mp->data.mtp_payload_u.wiggle_status.robot_id =
va_arg(args, int);
break;
default:
assert(0);
break;
......@@ -491,8 +499,19 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
}
break;
case MA_Status:
mp->data.mtp_payload_u.update_position.status =
va_arg(args, mtp_status_t);
switch (mp->data.opcode) {
case MTP_UPDATE_POSITION:
mp->data.mtp_payload_u.update_position.status =
va_arg(args, mtp_status_t);
break;
case MTP_WIGGLE_STATUS:
mp->data.mtp_payload_u.wiggle_status.status =
va_arg(args, mtp_status_t);
break;
default:
assert(0);
break;
}
break;
case MA_RequestID:
mp->data.mtp_payload_u.request_id.request_id =
......@@ -507,6 +526,10 @@ mtp_error_t mtp_init_packet(struct mtp_packet *mp, mtp_tag_t tag, ...)
mp->data.mtp_payload_u.telemetry.mtp_telemetry_u.garcia =
*(va_arg(args, struct mtp_garcia_telemetry *));
break;
case MA_WiggleType:
mp->data.mtp_payload_u.wiggle_request.wiggle_type =
va_arg(args, mtp_wiggle_t);
break;
}
tag = va_arg(args, mtp_tag_t);
......@@ -772,6 +795,48 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
}
break;
case MTP_WIGGLE_REQUEST:
fprintf(file,
" opcode:\twiggle-request\n"
" id:\t%d\n",
mp->data.mtp_payload_u.wiggle_request.robot_id
);
switch (mp->data.mtp_payload_u.wiggle_request.wiggle_type) {
case MTP_WIGGLE_180_R:
fprintf(file,
" wiggle_type:\t180deg right\n"
);
break;
case MTP_WIGGLE_180_R_L:
fprintf(file,
" wiggle_type:\t180deg right, 180deg left\n"
);
break;
case MTP_WIGGLE_360_R:
fprintf(file,
" wiggle_type:\t360deg right\n"
);
break;
case MTP_WIGGLE_360_R_L:
fprintf(file,
" wiggle_type:\t360deg right, 360deg left\n"
);
break;
default:
assert(0);
break;
}
break;
case MTP_WIGGLE_STATUS:
fprintf(file,
" opcode:\twiggle-status\n"
" id:\t%d\n"
" status:\t%d\n",
mp->data.mtp_payload_u.wiggle_status.robot_id,
mp->data.mtp_payload_u.wiggle_status.status
);
break;
default:
printf("%p %d\n", &mp->data.opcode, mp->data.opcode);
fprintf(stderr, "error: unknown opcode %d\n", mp->data.opcode);
......
......@@ -146,6 +146,7 @@ typedef enum {
MA_RequestID, /*< (int) */
MA_CommandID, /*< (int) */
MA_GarciaTelemetry, /*< (mtp_garcia_telemetry *) */
MA_WiggleType, /*< (mtp_wiggle_t) */
MA_TAG_MAX
} mtp_tag_t;
......
......@@ -104,6 +104,16 @@ enum mtp_opcode_t {
* Telemetry from a robot.
*/
MTP_TELEMETRY = 60,
/**
* vmcd tells emcd to wiggle a robot (for id purposes).
*/
MTP_WIGGLE_REQUEST = 70,
/**
* the result of a wiggle.
*/
MTP_WIGGLE_STATUS = 71,
MTP_OPCODE_MAX
......@@ -258,6 +268,27 @@ struct mtp_garcia_telemetry {
int user_led;
};
enum mtp_wiggle_t {
/* turn 180deg from current pos */
MTP_WIGGLE_180_R = 1,
/* turn 180deg from current pos, then 180deg in opposite direction */
MTP_WIGGLE_180_R_L = 2,
/* turn 360deg from current pos */
MTP_WIGGLE_360_R = 3,
/* turn 360deg from current pos, then 360deg in opposite direction */
MTP_WIGGLE_360_R_L = 4
};
struct mtp_wiggle_request {
int robot_id;
mtp_wiggle_t wiggle_type;
};
struct mtp_wiggle_status {
int robot_id;
mtp_status_t status;
};
union mtp_telemetry switch (mtp_robot_type_t type) {
case MTP_ROBOT_GARCIA: mtp_garcia_telemetry garcia;
};
......@@ -276,6 +307,8 @@ union mtp_payload switch (mtp_opcode_t opcode) {
case MTP_COMMAND_GOTO: mtp_command_goto command_goto;
case MTP_COMMAND_STOP: mtp_command_stop command_stop;
case MTP_TELEMETRY: mtp_telemetry telemetry;
case MTP_WIGGLE_REQUEST: mtp_wiggle_request wiggle_request;
case MTP_WIGGLE_STATUS: mtp_wiggle_status wiggle_status;
};
/**
......
......@@ -40,6 +40,7 @@ static void usage(void)
"Commands:\n"
" error notify init close request-position request-id\n"
" update-position update-id command-goto command-stop\n"
" wiggle-request wiggle-status\n"
"\n"
"Options:\n"
" -h\t\tPrint this message\n"
......@@ -61,6 +62,8 @@ static void usage(void)
" -r vmc|emc|rmc|emulab\n"
" \t\tThe role (Default: rmc)\n"
" -P #\t\tThe port the peer listening on\n"
" -W 180r|180rl|360r|360rl\n"
" \t\tThe wiggle type\n"
"\n"
"Example:\n"
" $ mtp_send -n garcia1 -P 3000 -i 1 -c 2 -m \"FooBar\" error\n");
......@@ -113,6 +116,7 @@ static int interpret_options(int *argcp, char ***argvp)
int role;
int port;
int wiggle_type;
} args = {
-1, /* id */
-1, /* code */
......@@ -129,6 +133,7 @@ static int interpret_options(int *argcp, char ***argvp)
NULL, /* message */
MTP_ROLE_RMC, /* role */
-1, /* port */
-1, /* wiggle type */
};
int argc = *argcp;
......@@ -141,7 +146,7 @@ static int interpret_options(int *argcp, char ***argvp)
/* Loop through the current set of command-line flags. */
while ((c = getopt(argc,
argv,
"hdwi:c:C:n:U:x:y:o:H:V:t:s:m:r:P:")) != -1) {
"hdwi:c:C:n:U:x:y:o:H:V:t:s:m:r:P:W:")) != -1) {
switch (c) {
case 'h':
usage();
......@@ -303,6 +308,27 @@ static int interpret_options(int *argcp, char ***argvp)
exit(1);
}
break;
case 'W':
if (strcasecmp(optarg,"180r") == 0) {
args.wiggle_type = MTP_WIGGLE_180_R;
}
else if (strcasecmp(optarg,"180rl") == 0) {
args.wiggle_type = MTP_WIGGLE_180_R_L;
}
else if (strcasecmp(optarg,"360r") == 0) {
args.wiggle_type = MTP_WIGGLE_360_R;
}
else if (strcasecmp(optarg,"360rl") == 0) {
args.wiggle_type = MTP_WIGGLE_360_R_L;
}
else {
fprintf(stderr,
"error: W option must be one of: 180r, 180rl, 360r, 360rl\n"
);
usage();
exit(1);
}
break;
default:
usage();
exit(1);
......@@ -353,6 +379,10 @@ static int interpret_options(int *argcp, char ***argvp)
opcode = MTP_COMMAND_GOTO;
else if (strcasecmp(argv[0], "command-stop") == 0)
opcode = MTP_COMMAND_STOP;
else if (strcasecmp(argv[0], "wiggle-request") == 0)
opcode = MTP_WIGGLE_REQUEST;
else if (strcasecmp(argv[0], "wiggle-status") == 0)
opcode = MTP_WIGGLE_STATUS;
else {
fprintf(stderr, "error: unknown command: %s\n", argv[0]);
usage();
......@@ -373,7 +403,8 @@ static int interpret_options(int *argcp, char ***argvp)
" timestamp:\t%f\n"
" status:\t%d\n"
" message:\t%s\n"
" port:\t\t%d\n",
" port:\t\t%d\n"
" wiggle-type:\t\t%d\n",
opcode,
args.id,
args.code,
......@@ -386,7 +417,8 @@ static int interpret_options(int *argcp, char ***argvp)
args.timestamp,
args.status,
args.message,
args.port);
args.port,
args.wiggle_type);
}
switch (opcode) {
......@@ -519,6 +551,36 @@ static int interpret_options(int *argcp, char ***argvp)
MA_TAG_DONE);
break;
case MTP_WIGGLE_REQUEST:
if (args.wiggle_type == -1)
required_option("W");
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_WiggleType, args.wiggle_type,
MA_TAG_DONE
);
break;
case MTP_WIGGLE_STATUS:
if (args.status == -1)
required_option("s");
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_Status, args.status,
MA_TAG_DONE
);
break;
default:
fprintf(stderr,
"internal error: command %s not supported at the moment...\n",
......
......@@ -73,6 +73,16 @@ int main(int argc, char *argv[])
4,
2,
};
static struct mtp_wiggle_request mwr = {
4,
1,
};
static struct mtp_wiggle_status mws = {
4,
4,
};
int fd, retval = EXIT_SUCCESS;
......@@ -110,6 +120,12 @@ int main(int argc, char *argv[])
assert(mp = mtp_make_packet(MTP_COMMAND_STOP, MTP_ROLE_VMC, &mcs));
assert(mtp_send_packet(fd,mp) == MTP_PP_SUCCESS);
assert(mp = mtp_make_packet(MTP_WIGGLE_REQUEST, MTP_ROLE_VMC, &mwr));
assert(mtp_send_packet(fd,mp) == MTP_PP_SUCCESS);
assert(mp = mtp_make_packet(MTP_WIGGLE_REQUEST, MTP_ROLE_VMC, &mws));
assert(mtp_send_packet(fd,mp) == MTP_PP_SUCCESS);
lseek(fd,0,SEEK_SET);
assert(mtp_receive_packet(fd,&mp) == MTP_PP_SUCCESS);
......
......@@ -108,6 +108,9 @@ struct gorobot_conn {
int gc_tries_remaining;
struct robot_position gc_actual_pos;
struct robot_position gc_intended_pos;
int goto_waiting;
int wiggle;
struct robot_position wiggle_pos;
};
static struct gorobot_conn gorobot_connections[128];
......@@ -300,6 +303,8 @@ static void handle_emc_packet(mtp_handle_t emc_handle)
struct mtp_command_stop *mcs = &mp.data.mtp_payload_u.command_stop;
struct mtp_update_position *mup =
&mp.data.mtp_payload_u.update_position;
struct mtp_wiggle_request *mwr = &mp.data.mtp_payload_u.wiggle_request;
struct mtp_wiggle_status *mws = &mp.data.mtp_payload_u.wiggle_status;
struct gorobot_conn *gc;
if (debug) {
......@@ -338,27 +343,149 @@ static void handle_emc_packet(mtp_handle_t emc_handle)
else {
struct mtp_packet smp;
info("gonna move\n");
if (!gc->wiggle) {
info("gonna move\n");
/*
* There is a new position, send a stop to gorobot so that
* it will send back an IDLE update and we can do more
* processing.
*/
gc->gc_intended_pos = mcg->position;
gc->gc_state = PS_PENDING_POSITION;
gc->gc_tries_remaining = 0;
mtp_init_packet(&smp,
MA_Opcode, MTP_COMMAND_STOP,
MA_Role, MTP_ROLE_RMC,
MA_RobotID, gc->gc_robot->id,
MA_CommandID, 1,
MA_TAG_DONE);
if (mtp_send_packet(gc->gc_handle, &smp)
!= MTP_PP_SUCCESS) {
error("cannot send reply packet\n");
}
mtp_free_packet(&smp);
}
else {
// queue up the requested posit
gc->goto_waiting = 1;
gc->gc_intended_pos = mcg->position;
}
}
break;
/*
* There is a new position, send a stop to gorobot so that it
* will send back an IDLE update and we can do more processing.
*/
gc->gc_intended_pos = mcg->position;
gc->gc_state = PS_PENDING_POSITION;
gc->gc_tries_remaining = 0;
case MTP_WIGGLE_REQUEST:
if ((gc = find_gorobot(mwr->robot_id)) == NULL) {
error ("unknown robot %d\n",mwr->robot_id);
// also need to send crap back to VMCD so that it doesn't
// wait for a valid wiggle complete msg
mtp_init_packet(&smp,
MA_Opcode, MTP_COMMAND_STOP,
mtp_init_packet(&mp,
MA_Opcode,MTP_WIGGLE_STATUS,
MA_Role,MTP_ROLE_RMC,
MA_RobotID,mwr->robot_id,
MA_Status,MTP_POSITION_STATUS_ERROR,
MA_TAG_DONE
);
if (mtp_send_packet(emc_handle, &mp) != MTP_PP_SUCCESS) {
error("cannot send reply packet\n");
}
}
else if (gc->wiggle) {
error("RECEIVED A WIGGLE when one is already in progess -- "
"there is no way to tell vmc what's going on!!!\n"
);
// XXX: try to do something better here, maybe add more
// more descriptive msgs... or maybe cut off the current
// wiggle and do the next one?
}
else {
// we're not already wiggling, so we send a stop to the robot
// and add some processing code in the gc handler.
gc->wiggle = 1;
if (mwr->wiggle_type == MTP_WIGGLE_180_R) {
gc->wiggle_pos.x = 0.0f;
gc->wiggle_pos.y = 0.0f;
gc->wiggle_pos.theta = M_PI;
// send a command stop to the robot, then handle
// in the gc_handle IDLE section
gc->gc_state = PS_PENDING_POSITION;
gc->gc_tries_remaining = 0;
mtp_init_packet(&mp,
MA_Opcode, MTP_COMMAND_STOP,
MA_Role, MTP_ROLE_RMC,
MA_RobotID, gc->gc_robot->id,
MA_CommandID, 1,
MA_TAG_DONE
);
if (mtp_send_packet(gc->gc_handle, &mp) != MTP_PP_SUCCESS)
{
error("cannot send stop packet to gc for wiggle\n");
// XXX: need more here, need to set gc->wiggle = 0; and
// notify vmc that its wiggle failed!
}
mtp_free_packet(&mp);
}
else {
// XXX: handle other wiggle types later!
error ("unknown wiggle type %d\n",mwr->wiggle_type);
mtp_init_packet(&mp,
MA_Opcode,MTP_WIGGLE_STATUS,
MA_Role,MTP_ROLE_RMC,
MA_RobotID,mwr->robot_id,
MA_Status,MTP_POSITION_STATUS_ERROR,
MA_TAG_DONE
);
if (mtp_send_packet(emc_handle, &mp) != MTP_PP_SUCCESS) {
error("cannot send reply packet\n");
}
mtp_free_packet(&mp);
}
}
break;
case MTP_WIGGLE_STATUS:
// if valid id and this gc is getting its wiggle on, we send the
// goto to gc->grobot, and wait for the call from grobot telling
// us that the goto finished.
if ((gc = find_gorobot(mws->robot_id)) == NULL) {
error("unknown robot %d in emc ack of wiggle-statusn",
mws->robot_id
);
}
else {
// send the gc->wiggle_pos as the goto data, to the correct
// robot
struct mtp_packet gmp;
info("actually starting wiggle for robot %d\n",
gc->gc_robot->id
);
mtp_init_packet(&gmp,
MA_Opcode, MTP_COMMAND_GOTO,
MA_Role, MTP_ROLE_RMC,
MA_Position, &(gc->wiggle_pos),
MA_RobotID, gc->gc_robot->id,
MA_CommandID, 1,
MA_TAG_DONE);
if (mtp_send_packet(gc->gc_handle, &smp) != MTP_PP_SUCCESS) {
error("cannot send reply packet\n");
MA_TAG_DONE
);
if (mtp_send_packet(gc->gc_handle, &gmp) !=
MTP_PP_SUCCESS) {
fatal("could not send actual wiggle to robot!");
// XXX: need to tell vmc about the problem!
}
mtp_free_packet(&smp);
}
break;
case MTP_COMMAND_STOP:
......@@ -371,6 +498,7 @@ static void handle_emc_packet(mtp_handle_t emc_handle)
/* Just stop the robot in its tracks and clear out any state */
gc->gc_state = PS_ARRIVED;
gc->gc_tries_remaining = 0;
gc->wiggle = 0;
mtp_init_packet(&smp,
MA_Opcode, MTP_COMMAND_STOP,
......@@ -490,9 +618,11 @@ static void handle_gc_packet(struct gorobot_conn *gc, mtp_handle_t emc_handle)
mtp_print_packet(stderr, &mp);
}
if ((mup->position.x != 0.0f) ||
(mup->position.y != 0.0f) ||
(mup->position.theta != 0.0f)) {
/* NOTE: we don't want to refine a wiggle! */
if (!(gc->wiggle) &&
((mup->position.x != 0.0f) ||
(mup->position.y != 0.0f) ||
(mup->position.theta != 0.0f))) {
struct robot_position new_pos;
struct mtp_packet ump;
......@@ -518,7 +648,31 @@ static void handle_gc_packet(struct gorobot_conn *gc, mtp_handle_t emc_handle)
switch (mup->status) {
case MTP_POSITION_STATUS_IDLE:
/* Response to a COMMAND_STOP. */
if (gc->gc_state == PS_PENDING_POSITION) {
if (gc->wiggle) {
// if we're gonna wiggle, we don't do the usual posit request
// here -- we just notify the vision system via emc that the
// robot is idle (i.e., is starting the wiggle)
struct mtp_packet vsn;
mtp_init_packet(&vsn,
MA_Opcode,MTP_WIGGLE_STATUS,
MA_Role,MTP_ROLE_RMC,
MA_RobotID,gc->gc_robot->id,
MA_Status,MTP_POSITION_STATUS_IDLE,
MA_TAG_DONE
);
if (mtp_send_packet(emc_handle,&vsn) != MTP_PP_SUCCESS) {
error("could not tell vmc that wiggle is starting...\n");
}
mtp_free_packet(&vsn);
// so now, we wait for emc to 'ack' our status msg after it
// notifies vmc. this will be our cue to get our wiggle on.
//( :-)
}
else if (gc->gc_state == PS_PENDING_POSITION) {
struct mtp_packet rmp;
info("starting move\n");
......@@ -560,13 +714,80 @@ static void handle_gc_packet(struct gorobot_conn *gc, mtp_handle_t emc_handle)
mtp_free_packet(&ump);
// XXX: ALSO NEED TO SEND A MSG TO VMC if this was a wiggle!!
mtp_init_packet(&ump,
MA_Opcode,MTP_WIGGLE_STATUS,
MA_Role,MTP_ROLE_RMC,
MA_RobotID,gc->gc_robot->id,
MA_Status,MTP_POSITION_STATUS_ERROR,
MA_TAG_DONE
);
if (mtp_send_packet(emc_handle, &ump) != MTP_PP_SUCCESS) {
fatal("wiggle-status error msg failed");
}
mtp_free_packet(&ump);
gc->wiggle = 0;
gc->gc_state = PS_ARRIVED;
}
break;
case MTP_POSITION_STATUS_COMPLETE:
info("gorobot finished\n");
if (gc->gc_state == PS_REFINING_POSITION) {
if (gc->wiggle) {