コード例 #1
0
ファイル: mtp_send.c プロジェクト: senseisimple/emulab-stable
/**
 * 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;
}
コード例 #2
0
ファイル: visionTrack.c プロジェクト: vnaybhat/emulab
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;
}