Tracker::~Tracker() { psmove_fusion_free(m_fusion); psmove_tracker_free(m_tracker); for (int i = 0; i < m_count; i++) { psmove_disconnect(m_moves[i]); } free(m_items); free(m_moves); }
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; }
void MoveHandler::disconnect() { logger->LogEvent("Disconnecting controller."); cvDestroyAllWindows(); this->frame = nullptr; for(int i = 0; i < this->connections; i++) { psmove_disconnect(this->controllers[i]); } psmove_tracker_free(this->tracker); }
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; }
MainWindow::~MainWindow() { psmove_tracker_free(m_tracker); psmove_disconnect(m_move); }
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; }
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; }