예제 #1
0
MainWindow::MainWindow(QWidget *parent) :
    QWidget(parent),
    m_mousePos(0, 0),
    m_mapping(),
    m_points(),
    m_pointsOffset(0),
    m_move(psmove_connect()),
    m_tracker(psmove_tracker_new()),
    m_timer(),
    m_path(),
    m_rect(rect()),
    m_rectOffset(0)
{
    m_points[0] = QPointF(1, 1);
    m_points[1] = QPointF(640, 0);
    m_points[2] = QPointF(640, 480);
    m_points[3] = QPointF(0, 480);
    m_mapping.set(m_points);

    setMouseTracking(true);

    if (m_move) {
        while (psmove_tracker_enable(m_tracker, m_move) != Tracker_CALIBRATED);
    }

    QObject::connect(&m_timer, SIGNAL(timeout()),
                     this, SLOT(timeout()));
    m_timer.start(1);
}
예제 #2
0
int main(int arg, char** args) {
    int count = psmove_count_connected();

    int i;
    void *frame;

    if (count == 0) {
        printf("No controllers connected.\n");
        return 1;
    }
    PSMove **moves = (PSMove **)calloc(count, sizeof(PSMove *));

    PSMoveTracker* tracker = psmove_tracker_new();

    for (i=0; i<count; i++) {
        moves[i] = psmove_connect_by_id(i);
        assert(moves[i] != NULL);

        while (psmove_tracker_enable(tracker, moves[i])
                != Tracker_CALIBRATED);
    }

    unsigned char r, g, b;
    psmove_tracker_get_camera_color(tracker, moves[0], &r, &g, &b);
    printf("Controller color: %02x%02x%02x\n", r, g, b);

    CvVideoWriter *writer = cvCreateVideoWriter("out.avi",
            CV_FOURCC('M','J','P','G'), 30, cvSize(640, 480), 1);

    while ((cvWaitKey(1) & 0xFF) != 27) {
        psmove_tracker_update_image(tracker);
        psmove_tracker_update(tracker, NULL);

        frame = psmove_tracker_get_frame(tracker);
        if (frame) {
            cvWriteFrame(writer, frame);
        }

        psmove_tracker_annotate(tracker);
        frame = psmove_tracker_get_frame(tracker);
        if (frame) {
            cvShowImage("live camera feed", frame);
        }
    }

    cvReleaseVideoWriter(&writer);

    for (i=0; i<count; i++) {
        psmove_disconnect(moves[i]);
    }

    psmove_tracker_free(tracker);
    free(moves);
    return 0;
}
예제 #3
0
Tracker::Tracker() : m_moves(NULL), m_count(psmove_count_connected()), mocapRecorder("psmovedata") {
    // PSMove *move;
    // move = psmove_connect();
    // if (move == NULL) {
    //     printf("Could not connect to default Move controller.\n"
    //            "Please connect one via USB or Bluetooth.\n");
    //     exit(1);
    // }
    // char *serial = psmove_get_serial(move);
    // auto ctype = psmove_connection_type(move);
    // switch (ctype) {
    //     case Conn_USB:
    //         printf("Connected via USB.\n");
    //         break;
    //     case Conn_Bluetooth:
    //         printf("Connected via Bluetooth.\n");
    //         break;
    //     case Conn_Unknown:
    //         printf("Unknown connection type.\n");
    //         break;
    // }
    std::cout << "PSMOVE : Initialising Tracker..." << std::endl;
    m_tracker = psmove_tracker_new();
    std::cout << "PSMOVE : Tracker Initialised" << std::endl;

    std::cout << "PSMOVE : Initialising Fusion..." << std::endl;
    m_fusion = psmove_fusion_new(m_tracker, 1., 1000.);
    std::cout << "PSMOVE : Fusion Initialised" << std::endl;

    psmove_tracker_set_mirror(m_tracker, PSMove_False);
    psmove_tracker_set_exposure(m_tracker, Exposure_HIGH);

    m_moves = (PSMove**) calloc(m_count, sizeof(PSMove*));
    m_items = (int*) calloc(m_count, sizeof(int));
    for (int i = 0; i < m_count; i++) {
        std::cout << "PSMOVE : Initialising Controller " << i << "..." << std::endl;
        m_moves[i] = psmove_connect_by_id(i);
        m_items[i] = WIRE_CUBE;

        psmove_enable_orientation(m_moves[i], PSMove_True);
        assert(psmove_has_orientation(m_moves[i]));


        std::cout << "PSMOVE : Calibrating Controller " << i << "..." << std::endl;
        while (psmove_tracker_enable(m_tracker, m_moves[i]) != Tracker_CALIBRATED)
            ;
        std::cout << "PSMOVE : Controller Calibrated " << i << "..." << std::endl;
    }
    // psmove_tracker_set_dimming(m_tracker,1);
    std::cout << "PSMOVE INITIALISED" << std::endl;
}
예제 #4
0
bool MoveHandler::connectTracker()
{
	std::stringstream string;

	logger->LogEvent("Initializeing tracker...");
	tracker = psmove_tracker_new();
	if(this->tracker == nullptr || this->tracker == NULL)
	{
		logger->LogEvent("Could not create tracker.\nAborting!");
		return false;
	}
	logger->LogEvent("OK");

	for(int i = 0; i < this->connections; i++)
	{	
		this->timer.start();
		while (*this->running && this->timer.secondsElapsed() < 60)
		{
			string.str("");
			string << "Calibrating connection #" << i;
			logger->LogEvent(string.str());
		

            if (psmove_tracker_enable(this->tracker, this->controllers[i]) == Tracker_CALIBRATED) 
            {
        		string.str("");
				string << "Enable automatic LED update for connection #" << i;
				logger->LogEvent(string.str());
				
				psmove_tracker_set_auto_update_leds(this->tracker, this->controllers[i], PSMove_True);
				break;
            }
            else
            {
				logger->LogEvent("Caibration failed. Retrying.");	
            }
        }

		logger->LogEvent("Calibration finished.");
		this->bulbHandler->startTimer();
	}

	logger->LogEvent("Enable tracker mirroring");
    psmove_tracker_set_mirror(this->tracker, PSMove_True);

	logger->LogEvent("Controller initialization finished");
	return true;
}
예제 #5
0
Tracker::Tracker()
    : m_moves(NULL),
      m_count(psmove_count_connected()),
      m_tracker(psmove_tracker_new()),
      m_fusion(psmove_fusion_new(m_tracker, 1., 1000.))
{
    psmove_tracker_set_mirror(m_tracker, PSMove_True);
    psmove_tracker_set_exposure(m_tracker, Exposure_HIGH);

    m_moves = (PSMove**)calloc(m_count, sizeof(PSMove*));
    m_items = (int*)calloc(m_count, sizeof(int));
    for (int i=0; i<m_count; i++) {
        m_moves[i] = psmove_connect_by_id(i);
        m_items[i] = WIRE_CUBE;

        psmove_enable_orientation(m_moves[i], PSMove_True);
        assert(psmove_has_orientation(m_moves[i]));

        while (psmove_tracker_enable(m_tracker, m_moves[i]) != Tracker_CALIBRATED);
    }
}
int
main(int arg, char** args)
{
    measurement measurements[MEASUREMENTS];
    float distance = MEASUREMENTS_CM_START;
    int pos = 0;

    PSMove *move = psmove_connect();

    if (move == NULL) {
        printf("Could not connect to the default controller.\n");
        return 1;
    }

    PSMoveTracker* tracker = psmove_tracker_new();

    if (tracker == NULL) {
        printf("Could not create tracker.\n");
        return 2;
    }

    printf("Calibrating controller...\n");
    while (psmove_tracker_enable(tracker, move) != Tracker_CALIBRATED);

    while (cvWaitKey(1) != 27 && pos < MEASUREMENTS) {
        psmove_tracker_update_image(tracker);
        psmove_tracker_update(tracker, NULL);
        printf("Distance: %.2f cm\n", distance);

        void *frame = psmove_tracker_get_image(tracker);
        cvShowImage("Camera", frame);

        unsigned char r, g, b;
        psmove_tracker_get_color(tracker, move, &r, &g, &b);
        psmove_set_leds(move, r, g, b);
        psmove_update_leds(move);

        float x, y, radius;
        psmove_tracker_get_position(tracker, move, &x, &y, &radius);

        unsigned int pressed, released;
        while (psmove_poll(move));
        psmove_get_button_events(move, &pressed, &released);
        if (pressed & Btn_CROSS) {
            // Save current measurement
            save(frame, (int)distance);
            measurements[pos].distance_cm = distance;
            measurements[pos].radius_px = radius;
            distance += MEASUREMENTS_CM_STEP;
            pos++;
        } else if (pressed & Btn_CIRCLE && pos > 0) {
            // Go back and retry previous measurement
            distance -= MEASUREMENTS_CM_STEP;
            pos--;
        }
    }

    int i;
    FILE *fp = fopen("distance.csv", "w");
    fprintf(fp, "distance,radius\n");
    for (i=0; i<pos; i++) {
        fprintf(fp, "%.5f,%.5f\n",
                measurements[i].distance_cm,
                measurements[i].radius_px);
    }
    fclose(fp);

    psmove_tracker_free(tracker);
    psmove_disconnect(move);

    return 0;
}
예제 #7
0
uint32 FPSMoveWorker::Run()
{
    if (!psmove_init(PSMOVE_CURRENT_VERSION))
    {
        UE_LOG(LogPSMove, Error, TEXT("PS Move API init failed (wrong version?)"));
        return -1;
    }
    // I want the psmoves and psmove_tracker to be local variables in the thread.
    
    // Initialize an empty array of psmove controllers
	PSMove* psmoves[FPSMoveWorker::k_max_controllers];
	memset(psmoves, 0, sizeof(psmoves));

    // Initialize and configure the psmove_tracker
    PSMoveTracker *psmove_tracker = psmove_tracker_new(); // Unfortunately the API does not have a way to change the resolution and framerate.
    PSMoveFusion *psmove_fusion = psmove_fusion_new(psmove_tracker, 1., 1000.);
    int tracker_width = 640;
    int tracker_height = 480;
    if (psmove_tracker)
    {
        UE_LOG(LogPSMove, Log, TEXT("PSMove tracker initialized."));
        
        //Set exposure. TODO: Expose this to component.
        psmove_tracker_set_exposure(psmove_tracker, Exposure_LOW);  //Exposure_LOW, Exposure_MEDIUM, Exposure_HIGH
        psmove_tracker_set_smoothing(psmove_tracker, 0, 1);
		psmove_tracker_set_mirror(psmove_tracker, PSMove_True);
        
        psmove_tracker_get_size(psmove_tracker, &tracker_width, &tracker_height);
        UE_LOG(LogPSMove, Log, TEXT("Camera Dimensions: %d x %d"), tracker_width, tracker_height);
    }
    else {
        UE_LOG(LogPSMove, Log, TEXT("PSMove tracker failed to initialize."));
    }
    
    //Initial wait before starting.
    FPlatformProcess::Sleep(0.03);

    float xcm, ycm, zcm, oriw, orix, oriy, oriz;
    while (StopTaskCounter.GetValue() == 0)
    {        
        // Get positional data from tracker
        if (psmove_tracker)
        {            
			// Setup or tear down controller connections based on the number of active controllers
			UpdateControllerConnections(psmove_tracker, psmoves);

			// Renew the image on camera
			if (PSMoveCount > 0)
			{
				psmove_tracker_update_image(psmove_tracker); // Sometimes libusb crashes here.
				psmove_tracker_update_cbb(psmove_tracker, NULL); // Passing null (instead of m_moves[i]) updates all controllers.
			}
		}
		else {
			FPlatformProcess::Sleep(0.001);
		}

		for (int i = 0; i < PSMoveCount; i++)
		{
			FPSMoveRawControllerData_TLS &localControllerData = WorkerControllerDataArray[i];

			//--------------
			// Read the published data from the component
			//--------------
			localControllerData.WorkerRead();

			// Get positional data from tracker
			if (psmove_tracker)
            {
                localControllerData.IsTracked = psmove_tracker_get_status(psmove_tracker, psmoves[i]) == Tracker_TRACKING;

                psmove_fusion_get_transformed_location(psmove_fusion, psmoves[i], &xcm, &ycm, &zcm);

                if (localControllerData.IsTracked &&
                    xcm && ycm && zcm &&
                    !isnan(xcm) && !isnan(ycm) && !isnan(zcm) &&
                    xcm == xcm && ycm == ycm && zcm == zcm)
                {
                    localControllerData.PosX = xcm;
                    localControllerData.PosY = ycm;
                    localControllerData.PosZ = zcm;
                }
                else {
                    localControllerData.IsTracked = false;
                }

                //UE_LOG(LogPSMove, Log, TEXT("X: %f, Y: %f, Z: %f"), xcm, ycm, zcm);
                if (localControllerData.ResetPoseRequest && localControllerData.IsTracked)
                {
                    psmove_tracker_reset_location(psmove_tracker, psmoves[i]);
                }

                // If we are to change the tracked colour.
                if (localControllerData.UpdateLedRequest)
                {
                    // Stop tracking the controller with the existing color
                    psmove_tracker_disable(psmove_tracker, psmoves[i]);
                    localControllerData.IsTracked = false;
                    localControllerData.IsCalibrated = false;

                    psmove_tracker_set_dimming(psmove_tracker, 0.0);  // Set dimming to 0 to trigger blinking calibration.
                    psmove_set_leds(psmoves[i], 0, 0, 0);  // Turn off the LED to make sure it isn't trackable until new colour set.
                    psmove_update_leds(psmoves[i]);
                    FColor newFColor = localControllerData.LedColourRequest.Quantize();

                    if (psmove_tracker_enable_with_color(psmove_tracker, psmoves[i], newFColor.R, newFColor.G, newFColor.B) == Tracker_CALIBRATED)
                    {
                        this->WorkerControllerDataArray[i].IsCalibrated = true;
                    }
                    else
                    {
                        UE_LOG(LogPSMove, Error, TEXT("Failed to change tracking color for PSMove controller %d"), i);
                    }

                    localControllerData.LedColourWasUpdated = true;
                }
                else
                {
                    localControllerData.LedColourWasUpdated = false;
                }

			}
			else {
				FPlatformProcess::Sleep(0.001);
			}

			// Do bluetooth IO: Orientation, Buttons, Rumble
			
            //TODO: Is it necessary to keep polling until no frames are left?
            while (psmove_poll(psmoves[i]) > 0)
            {
                // Update the controller status (via bluetooth)
                psmove_poll(psmoves[i]);

                // Get the controller orientation (uses IMU).
                psmove_get_orientation(psmoves[i],
                    &oriw, &orix, &oriy, &oriz);
                localControllerData.OriW = oriw;
                localControllerData.OriX = orix;
                localControllerData.OriY = oriy;
                localControllerData.OriZ = oriz;
                //UE_LOG(LogPSMove, Log, TEXT("Ori w,x,y,z: %f, %f, %f, %f"), oriw, orix, oriy, oriz);

                // Get the controller button state
                localControllerData.Buttons = psmove_get_buttons(psmoves[i]);  // Bitwise; tells if each button is down.
                psmove_get_button_events(psmoves[i], &localControllerData.Pressed, &localControllerData.Released);  // i.e., state change

                // Get the controller trigger value (uint8; 0-255)
                localControllerData.TriggerValue = psmove_get_trigger(psmoves[i]);

                // Set the controller rumble (uint8; 0-255)
                psmove_set_rumble(psmoves[i], localControllerData.RumbleRequest);
            }

            if (localControllerData.ResetPoseRequest)
            {
                psmove_reset_orientation(psmoves[i]);
                //TODO: reset yaw only
                localControllerData.PoseWasReset = true;
            }
            else {
                localControllerData.PoseWasReset = false;
            }


			//--------------
			// Publish the updated worker data to the component
			//--------------
			localControllerData.WorkerPost();
        }    

        //Sleeping the thread seems to crash libusb.
        //FPlatformProcess::Sleep(0.005);        
    }
    
    // Delete the controllers
    for (int i = 0; i<PSMoveCount; i++)
    {
        psmove_disconnect(psmoves[i]);
    }

    // Delete the fusion
    if (psmove_fusion)
    {
        psmove_fusion_free(psmove_fusion);
    }
    
    // Delete the tracker
    if (psmove_tracker)
    {
        psmove_tracker_free(psmove_tracker);
    }

    psmove_shutdown();

    return 0;
}
예제 #8
0
int main(int arg, char** args) {
    int i;
    int count = psmove_count_connected();
    PSMove* controllers[count];

    printf("%s", "### Trying to init PSMoveTracker...");
    PSMoveTracker* tracker = psmove_tracker_new();
    printf("%s\n", "OK");
    printf("### Found %d controllers.\n", count);

    void *frame;
    unsigned char r, g, b;
    int result;

    for (i=0; i<count; i++) {
        printf("Opening controller %d\n", i);
        controllers[i] = psmove_connect_by_id(i);
        assert(controllers[i] != NULL);

        while (1) {
            printf("Calibrating controller %d...", i);
            fflush(stdout);
            result = psmove_tracker_enable(tracker, controllers[i]);

            if (result == Tracker_CALIBRATED) {
                printf("OK\n");
                break;
            } else {
                printf("ERROR - retrying\n");
            }
        }

    }

    while ((cvWaitKey(1) & 0xFF) != 27) {
        psmove_tracker_update_image(tracker);
        psmove_tracker_update(tracker, NULL);

        frame = psmove_tracker_get_image(tracker);
        if (frame) {
            cvShowImage("live camera feed", frame);
        }

        for (i=0; i<count; i++) {
            psmove_tracker_get_color(tracker, controllers[i], &r, &g, &b);
            psmove_set_leds(controllers[i], r, g, b);
            psmove_update_leds(controllers[i]);

            float x, y, r;
            psmove_tracker_get_position(tracker, controllers[i], &x, &y, &r);
            printf("x: %10.2f, y: %10.2f, r: %10.2f\n", x, y, r);
        }
    }

    for (i=0; i<count; i++) {
        psmove_disconnect(controllers[i]);
    }

    psmove_tracker_free(tracker);
    return 0;
}