/**
* Interpret a single set of command-line flags using getopt, then construct an
* MTP packet, send it to the server, and optionally wait for a reply. This
* function must be called multiple times to handle subsequent packets whose
* flags are seperated by a double-dash "--".
*
* @param argcp Pointer to main's argc argument.
* @param argvp Pointer to main's argv argument.
* @return Zero on success, an error code otherwise.
*/
static int interpret_options(int *argcp, char ***argvp)
{
static mtp_handle_t mh = NULL;
static struct {
int id;
int code;
char *hostname;
char *path;
int command_id;
float x;
float y;
float orientation;
float horizontal;
float vertical;
float speed;
double timestamp;
int status;
char *message;
int role;
double vleft;
double vright;
int port;
int wiggle_type;
} args = {
-1, /* id */
-1, /* code */
NULL, /* hostname */
NULL, /* path */
-1, /* command_id */
DBL_MIN, /* x */
DBL_MIN, /* y */
DBL_MIN, /* orientation */
DBL_MIN, /* horizontal */
DBL_MIN, /* vertical */
0.2, /* speed */
-1.0, /* timestamp */
-1, /* status */
NULL, /* message */
MTP_ROLE_RMC, /* role */
DBL_MIN,
DBL_MIN,
-1, /* port */
-1, /* wiggle type */
};
int argc = *argcp;
char **argv = *argvp;
int c, waitmode = 0, retval = EXIT_SUCCESS;
mtp_packet_t mp;
char opcode;
/* 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:S:W:L:R:D:")) != -1) {
switch (c) {
case 'h':
usage();
exit(0);
break;
case 'd':
debug += 1;
break;
case 'w':
waitmode = 1;
break;
case 'i':
if (sscanf(optarg, "%d", &args.id) != 1) {
fprintf(stderr,
"error: i option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'c':
if (sscanf(optarg, "%d", &args.code) != 1) {
fprintf(stderr,
"error: c option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'C':
if (sscanf(optarg, "%d", &args.command_id) != 1) {
fprintf(stderr,
"error: C option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'n':
if (strlen(optarg) == 0) {
fprintf(stderr, "error: n option is empty\n");
usage();
exit(1);
}
args.hostname = optarg;
break;
case 'U':
if (strlen(optarg) == 0) {
fprintf(stderr, "error: U option is empty\n");
usage();
exit(1);
}
args.path = optarg;
break;
case 'x':
if (sscanf(optarg, "%f", &args.x) != 1) {
fprintf(stderr,
"error: x option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'y':
if (sscanf(optarg, "%f", &args.y) != 1) {
fprintf(stderr,
"error: y option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'o':
if (sscanf(optarg, "%f", &args.orientation) != 1) {
fprintf(stderr,
"error: o option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'H':
if (sscanf(optarg, "%f", &args.horizontal) != 1) {
fprintf(stderr,
"error: H option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'V':
if (sscanf(optarg, "%f", &args.vertical) != 1) {
fprintf(stderr,
"error: V option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 't':
if (sscanf(optarg, "%lf", &args.timestamp) != 1) {
fprintf(stderr,
"error: t option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 's':
if (strcasecmp(optarg, "idle") == 0) {
args.status = MTP_POSITION_STATUS_IDLE;
}
else if (strcasecmp(optarg, "moving") == 0) {
args.status = MTP_POSITION_STATUS_MOVING;
}
else if (strcasecmp(optarg, "error") == 0) {
args.status = MTP_POSITION_STATUS_ERROR;
}
else if (strcasecmp(optarg, "complete") == 0) {
args.status = MTP_POSITION_STATUS_COMPLETE;
}
else {
fprintf(stderr,
"error: s option must be one of: idle, moving, "
"error, or complete\n");
usage();
exit(1);
}
break;
case 'm':
args.message = optarg;
break;
case 'r':
if (strcasecmp(optarg, "vmc") == 0) {
args.role = MTP_ROLE_VMC;
}
else if (strcasecmp(optarg, "emc") == 0) {
args.role = MTP_ROLE_EMC;
}
else if (strcasecmp(optarg, "rmc") == 0) {
args.role = MTP_ROLE_RMC;
}
else if (strcasecmp(optarg, "emulab") == 0) {
args.role = MTP_ROLE_EMULAB;
}
else {
fprintf(stderr,
"error: r option must be one of: vmc, emc, or rmc\n");
usage();
exit(1);
}
break;
case 'P':
if (sscanf(optarg, "%d", &args.port) != 1) {
fprintf(stderr,
"error: t option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'S':
if (sscanf(optarg, "%f", &args.speed) != 1) {
fprintf(stderr,
"error: S option is not a number: %s\n",
optarg);
usage();
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;
case 'L':
if (sscanf(optarg, "%lf", &args.vleft) != 1) {
fprintf(stderr,
"error: L option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'R':
if (sscanf(optarg, "%lf", &args.vright) != 1) {
fprintf(stderr,
"error: R option is not a number: %s\n",
optarg);
usage();
exit(1);
}
break;
case 'D':
{
int delay;
if (sscanf(optarg, "%d", &delay) != 1) {
fprintf(stderr,
"error: S option is not a number: %s\n",
optarg);
usage();
exit(1);
}
sleep(delay);
}
break;
default:
fprintf(stderr, "error: bad option -- %c\n", c);
usage();
exit(1);
break;
}
}
argc -= optind;
argv += optind;
if (argc == 0) {
fprintf(stderr, "error: missing command name argument\n");
usage();
exit(1);
}
*argcp = argc - 1;
*argvp = argv + 1;
if (args.hostname == NULL && args.path == NULL) {
required_option("n");
}
if (args.port == -1 && args.path == NULL) {
required_option("P");
}
if (strcasecmp(argv[0], "error") == 0)
opcode = MTP_CONTROL_ERROR;
else if (strcasecmp(argv[0], "notify") == 0)
opcode = MTP_CONTROL_NOTIFY;
else if (strcasecmp(argv[0], "init") == 0)
opcode = MTP_CONTROL_INIT;
else if (strcasecmp(argv[0], "close") == 0)
opcode = MTP_CONTROL_CLOSE;
else if (strcasecmp(argv[0], "config-vmc") == 0)
opcode = MTP_CONFIG_VMC;
else if (strcasecmp(argv[0], "config-rmc") == 0)
opcode = MTP_CONFIG_RMC;
else if (strcasecmp(argv[0], "request-position") == 0)
opcode = MTP_REQUEST_POSITION;
else if (strcasecmp(argv[0], "request-id") == 0)
opcode = MTP_REQUEST_ID;
else if (strcasecmp(argv[0], "update-position") == 0)
opcode = MTP_UPDATE_POSITION;
else if (strcasecmp(argv[0], "update-id") == 0)
opcode = MTP_UPDATE_ID;
else if (strcasecmp(argv[0], "command-goto") == 0)
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 if (strcasecmp(argv[0], "request-report") == 0)
opcode = MTP_REQUEST_REPORT;
else if (strcasecmp(argv[0], "command-wheels") == 0)
opcode = MTP_COMMAND_WHEELS;
else {
fprintf(stderr, "error: unknown command: %s\n", argv[0]);
usage();
exit(1);
}
if (debug > 0) {
fprintf(stderr,
"Opcode: %d\n"
" id:\t\t%d\n"
" code:\t\t%d\n"
" hostname:\t%s\n"
" x:\t\t%f\n"
" y:\t\t%f\n"
" orientation:\t%f\n"
" horizontal:\t%f\n"
" vertical:\t%f\n"
" timestamp:\t%f\n"
" status:\t%d\n"
" message:\t%s\n"
" port:\t\t%d\n"
" wiggle-type:\t\t%d\n",
opcode,
args.id,
args.code,
args.hostname,
args.x,
args.y,
args.orientation,
args.horizontal,
args.vertical,
args.timestamp,
args.status,
args.message,
args.port,
args.wiggle_type);
}
switch (opcode) {
case MTP_CONTROL_ERROR:
case MTP_CONTROL_NOTIFY:
case MTP_CONTROL_INIT:
case MTP_CONTROL_CLOSE:
if (args.id == -1)
required_option("i");
if (args.code == -1)
required_option("c");
if (args.message == NULL)
required_option("m");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_ID, args.id,
MA_Code, args.code,
MA_Message, args.message,
MA_TAG_DONE);
break;
case MTP_CONFIG_VMC:
case MTP_CONFIG_RMC:
fprintf(stderr, "internal error: not supported at the moment...\n");
assert(0);
break;
case MTP_REQUEST_POSITION:
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_TAG_DONE);
break;
case MTP_REQUEST_ID:
if (args.x == DBL_MIN)
required_option("x");
if (args.y == DBL_MIN)
required_option("y");
if (args.orientation == DBL_MIN)
required_option("o");
if (args.timestamp == -1.0)
required_option("t");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_X, args.x,
MA_Y, args.y,
MA_Theta, args.orientation,
MA_Timestamp, args.timestamp,
MA_TAG_DONE);
break;
case MTP_UPDATE_POSITION:
if (args.id == -1)
required_option("i");
if (args.x == DBL_MIN)
required_option("x");
if (args.y == DBL_MIN)
required_option("y");
if (args.orientation == DBL_MIN)
required_option("o");
if (args.status == -1)
required_option("s");
if (args.timestamp == -1.0)
required_option("t");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_X, args.x,
MA_Y, args.y,
MA_Theta, args.orientation,
MA_Timestamp, args.timestamp,
MA_Status, args.status,
MA_TAG_DONE);
break;
case MTP_UPDATE_ID:
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_TAG_DONE);
break;
case MTP_COMMAND_GOTO:
if (args.id == -1)
required_option("i");
if (args.x == DBL_MIN)
required_option("x");
if (args.y == DBL_MIN)
required_option("y");
if (args.orientation == DBL_MIN)
required_option("o");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_CommandID, args.command_id,
MA_RobotID, args.id,
MA_X, args.x,
MA_Y, args.y,
MA_Theta, args.orientation,
MA_Timestamp, args.timestamp,
MA_Speed, args.speed,
MA_TAG_DONE);
break;
case MTP_COMMAND_STOP:
if (args.id == -1)
required_option("i");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_CommandID, args.command_id,
MA_RobotID, args.id,
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;
case MTP_REQUEST_REPORT:
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_TAG_DONE);
break;
case MTP_COMMAND_WHEELS:
if (args.id == -1)
required_option("i");
if (args.vleft == DBL_MIN)
required_option("L");
if (args.vright == DBL_MIN)
required_option("R");
mtp_init_packet(&mp,
MA_Opcode, opcode,
MA_Role, args.role,
MA_RobotID, args.id,
MA_vleft, args.vleft,
MA_vright, args.vright,
MA_TAG_DONE);
break;
default:
fprintf(stderr,
"internal error: command %s not supported at the moment...\n",
argv[0]);
assert(0);
break;
}
if (debug) {
mtp_print_packet(stdout, &mp);
}
if (mh == NULL) {
mh = mtp_create_handle2(args.hostname, args.port, args.path);
}
if (mh == NULL) {
fprintf(stderr, "error: could not connect to server\n");
exit(1);
}
else if (mtp_send_packet(mh, &mp) != MTP_PP_SUCCESS) {
perror("mtp_send_packet"); // XXX not right
}
else {
if (debug) {
mtp_print_packet(stdout, &mp);
}
if (waitmode) {
struct mtp_packet mp_reply;
if (mtp_receive_packet(mh, &mp_reply) != MTP_PP_SUCCESS) {
perror("mtp_receive_packet");
}
else {
mtp_print_packet(stdout, &mp_reply);
mtp_free_packet(&mp_reply);
}
}
}
#if !defined(linux)
/*
* Important, must inform getopt that it needs to reset its internal
* state.
*/
optreset = 1;
#endif
optind = 1;
return retval;
}
int vtUpdate(struct lnMinList *now,
struct vmc_client *vc,
struct mtp_packet *mp,
struct lnMinList *pool)
{
int retval = 0;
assert(now != NULL);
assert(vc != NULL);
assert(mp != NULL);
assert(pool != NULL);
if (debug > 2) {
mtp_print_packet(stdout, mp);
fflush(stdout);
}
switch (mp->data.opcode) {
case MTP_CONTROL_ERROR:
retval = 1;
break;
case MTP_UPDATE_POSITION:
{
struct mtp_update_position *mup;
struct vision_track *vt;
mup = &mp->data.mtp_payload_u.update_position;
vt = (struct vision_track *)lnRemHead(pool);
assert(vt != NULL);
vt->vt_position = mup->position;
vt->vt_client = vc;
vt->vt_age = 0;
vt->vt_userdata = NULL;
/* zero out the smoothing data for this
* track!
*/
//vt->ma.oldest_index = 0;
//vt->ma.number_valid_positions = 0;
lnAddTail(now, &vt->vt_link);
/* Adjust the cameras viewable area based on this track. */
if (mup->position.x < vc->vc_left)
vc->vc_left = mup->position.x;
if (mup->position.x > vc->vc_right)
vc->vc_right = mup->position.x;
if (mup->position.y < vc->vc_top)
vc->vc_top = mup->position.y;
if (mup->position.y > vc->vc_bottom)
vc->vc_bottom = mup->position.y;
retval = (mup->status == MTP_POSITION_STATUS_CYCLE_COMPLETE);
}
break;
default:
error("unhandled vmc-client packet %d\n", mp->data.opcode);
break;
}
return retval;
}
