void* capture(void* arg) { int res; int i = 0; CameraFile* file; pthread_cleanup_push(cleanup, NULL); while(!global->stop) { unsigned long int xsize; const char* xdata; pthread_mutex_lock(&control_mutex); res = gp_file_new(&file); CAMERA_CHECK_GP(res, "gp_file_new"); res = gp_camera_capture_preview(camera, file, context); CAMERA_CHECK_GP(res, "gp_camera_capture_preview"); pthread_mutex_lock(&global->in[plugin_id].db); res = gp_file_get_data_and_size(file, &xdata, &xsize); if(xsize == 0) { if(i++ > 3) { IPRINT("Restarted too many times; giving up\n"); return NULL; } int value = 0; IPRINT("Read 0 bytes from camera; restarting it\n"); camera_set("capture", &value); sleep(3); value = 1; camera_set("capture", &value); } else i = 0; CAMERA_CHECK_GP(res, "gp_file_get_data_and_size"); memcpy(global->in[plugin_id].buf, xdata, xsize); res = gp_file_unref(file); pthread_mutex_unlock(&control_mutex); CAMERA_CHECK_GP(res, "gp_file_unref"); global->in[plugin_id].size = xsize; DBG("Read %d bytes from camera.\n", global->in[plugin_id].size); pthread_cond_broadcast(&global->in[plugin_id].db_update); pthread_mutex_unlock(&global->in[plugin_id].db); usleep(delay); } pthread_cleanup_pop(1); return NULL; }
int input_cmd(int plugin, unsigned int control_id, unsigned int group, int value) { int res; int i; DBG("Requested cmd (id: %d) for the %d plugin. Group: %d value: %d\n", control_id, plugin_id, group, value); switch(group) { case IN_CMD_GENERIC: for(i = 0; i < global->in[plugin_id].parametercount; i++) { if((global->in[plugin_id].in_parameters[i].ctrl.id == control_id) && (global->in[plugin_id].in_parameters[i].group == IN_CMD_GENERIC)) { DBG("Generic control found (id: %d): %s\n", control_id, global->in[plugin_id].in_parameters[i].ctrl.name); if(control_id == 1) { float z = value; pthread_mutex_lock(&control_mutex); res = camera_set("zoom", &z); pthread_mutex_unlock(&control_mutex); } DBG("New %s value: %d\n", global->in[plugin_id].in_parameters[i].ctrl.name, value); return 0; } } DBG("Requested generic control (%d) did not found\n", control_id); return -1; break; } return 0; }
Camera *Node::camera_create(Assets &assets) { std::unique_ptr<Camera> camera(new Camera()); Camera *camera_ptr = camera.get(); camera_set(camera_ptr); assets.camera_add(std::move(camera)); return camera_ptr; }
void cleanup(void *arg) { int value = 0; // TODO check to see if we have already cleaned up? IPRINT("PTP2 capture - Cleaning up\n"); camera_set("capture", &value); gp_camera_exit(camera, context); gp_camera_unref(camera); gp_context_unref(context); free(global->in[plugin_id].buf); }
/*** Main Triangulation Function ***/ int main() { initialize_api(); // Initializes the robot API robot_connect("127.0.0.1"); // This always needs to be 127.0.0.1 to connect to the robot barcode_configure(2 /*number of digits*/, 10 /*barcodes per pass*/, 2 /*kept passes*/, 2 /*skip pixel width*/, 2 /*min bar width*/, 100 /* Allowed skew */, -1 /*Otsu thresholding*/, image_width /*image width*/, image_height /*image height*/); sleep(1); int** barcodes; // Pointer for a two-dimensional array, will hold returned barcodes int* xy; // Pointer to an array [x,y] camera_set(CAM_ANGLE); // Sets the tilt on the camera to the current defined camera angle // Create a thread to handle grabbing images for checking barcodes pthread_create(&vision_thread, NULL, barcode_start, NULL); // The final x and y coordinates of the three barcodes double x1,y1,x2,y2,x3,y3; x1 = y1 = x2 = y2 = x3 = y3 = 0.0; // Grab the current seen barcodes barcode_frame_wait(); // Waits for the next frame from the current counter barcodes = barcode_get_barcodes(); // Retrieves the barcodes in the 2D array int num_codes = (int)barcodes[0]; // Number of barcodes detected int num_digits = (int)barcodes[1]; // Number of digits per barcode (will be 2) printf("Detected %d barcodes %d digits long:\n", num_codes, num_digits); // Find three barcodes if we didn't see three at first while (num_codes < 3) { // Code to shutdown when the high bump is pressed. Just in case we never find the cans... if (get_high_bump() == 1) { free(barcodes); robot_stop(); /*** Teardown Code ***/ vision_running = 0; // Tells the background thread to nicely finish pthread_join(vision_thread, NULL); // Waits nicely for the barcode module to shutdown shutdown_api(); // Shuts down the robot API return 0; // Returns 0 } printf("Didn't see 3 barcodes. Moving to find them...\n\n"); random_move((int)barcodes[0]); free(barcodes); sleep(3); barcode_frame_wait_start(); // Reset the frame wait counter to wait for next full frame barcode_frame_wait(); // Waits for the next frame from the current counter barcodes = barcode_get_barcodes(); // Retrieves the barcodes in the 2D array num_codes = (int)barcodes[0]; // Number of barcodes detected num_digits = (int)barcodes[1]; // Number of digits per barcode (will be 2) printf("Detected %d barcodes %d digits long:\n", num_codes, num_digits); } int i; /* Runs through the barcodes detected, which start at index [2] */ for(i = 2; i < num_codes+2; i++) { /* This block converts the raw digits into actual numbers */ int j; uint32_t sum = 0; for(j = 0; j < num_digits; j++) { sum += barcodes[i][j] * pow(10,(num_digits-1)-j); } /* Get the X and Y positions of the cans relative to the robot */ xy = barcode_get_cur_xy(sum); int bottom_y = convert_camera_y(barcodes[i][j+3]); double x_pos = can_xpos(bottom_y); double y_pos = can_ypos(x_pos,convert_camera_x(xy[0])); printf("Barcode: %d\tX_CAM: %d\tY_CAM: %d\n",sum,convert_camera_x(xy[0]),bottom_y); printf("X_POS: %f\n",x_pos); printf("Y_POS: %f\n",y_pos); // Save the (x,y) coordinates in the proper variables for use later switch(i - 1) { case 1: x1 = x_pos; y1 = y_pos; break; case 2: x2 = x_pos; y2 = y_pos; break; case 3: x3 = x_pos; y3 = y_pos; break; default: break; } free(xy); } // We don't need the camera or barcodes any more, so free them up vision_running = 0; // Tells the background thread to nicely finish pthread_join(vision_thread, NULL); // Waits nicely for the barcode module to shutdown free(barcodes); /*** Calculate the centroid, and the distance and angle of rotation to it ***/ double x_centroid = (x1 + x2 + x3) / 3.0; double y_centroid = (y1 + y2 + y3) / 3.0; double dist_centroid = sqrt((x_centroid * x_centroid) + (y_centroid * y_centroid)); int16_t dist_cm = (int16_t) round(dist_centroid); double theta_centroid = atan((y_centroid / x_centroid)); int16_t theta_deg = (int16_t) round(DEG_CONV * (theta_centroid * calculate_error_damp(dist_centroid))); printf("\n\nCentroid X: %f\nCentroid Y: %f\nDistance: %f\nAngle: %d\n\n",x_centroid,y_centroid,dist_centroid,theta_deg); // Use the distance and angle calculations to set the proper robot turn and move values int8_t speed; if(theta_deg >= 0){ speed = ROBOT_SPEED; } else{ // If the angle is negative, turn clockwise (= negative speed) speed = ROBOT_SPEED * (-1); theta_deg = theta_deg*(-1); // Angle must be positive for turn_robot_wait() } printf("Turning by %d degrees\n", theta_deg); turn_robot_wait(speed,theta_deg); printf("Moving %d cm\n",dist_cm); move_distance_wait(ROBOT_SPEED,dist_cm); // Make sure the robot has stopped robot_stop(); /*** Teardown Code ***/ shutdown_api(); // Shuts down the robot API return 0; // Returns 0 }
Eigen::Matrix3Xd GraspSet::calculateShadow4(const PointList& point_list, double shadow_length) const { double voxel_grid_size = 0.003; // voxel size for points that fill occluded region double num_shadow_points = floor(shadow_length / voxel_grid_size); // number of points along each shadow vector const int num_cams = point_list.getCamSource().rows(); // Calculate the set of cameras which see the points. Eigen::VectorXi camera_set = point_list.getCamSource().rowwise().sum(); // Calculate the center point of the point neighborhood. Eigen::Vector3d center = point_list.getPoints().rowwise().sum(); center /= (double) point_list.size(); // Stores the list of all bins of the voxelized, occluded points. std::vector< Vector3iSet > shadows; shadows.resize(num_cams, Vector3iSet(num_shadow_points * 10000)); for (int i = 0; i < num_cams; i++) { if (camera_set(i) >= 1) { double t0_if = omp_get_wtime(); // Calculate the unit vector that points from the camera position to the center of the point neighborhood. Eigen::Vector3d shadow_vec = center - point_list.getViewPoints().col(i); // Scale that vector by the shadow length. shadow_vec = shadow_length * shadow_vec / shadow_vec.norm(); // Calculate occluded points. // shadows[i] = calculateVoxelizedShadowVectorized4(point_list, shadow_vec, num_shadow_points, voxel_grid_size); calculateVoxelizedShadowVectorized(point_list.getPoints(), shadow_vec, num_shadow_points, voxel_grid_size, shadows[i]); } } // Only one camera view point. if (num_cams == 1) { // Convert voxels back to points. // std::vector<Eigen::Vector3i> voxels(shadows[0].begin(), shadows[0].end()); // Eigen::Matrix3Xd shadow_out = shadowVoxelsToPoints(voxels, voxel_grid_size); // return shadow_out; return shadowVoxelsToPoints(std::vector<Eigen::Vector3i>(shadows[0].begin(), shadows[0].end()), voxel_grid_size); } // Multiple camera view points: find the intersection of all sets of occluded points. double t0_intersection = omp_get_wtime(); Vector3iSet bins_all = shadows[0]; for (int i = 1; i < num_cams; i++) { if (camera_set(i) >= 1) // check that there are points seen by this camera { bins_all = intersection(bins_all, shadows[i]); } } if (MEASURE_TIME) std::cout << "intersection runtime: " << omp_get_wtime() - t0_intersection << "s\n"; // Convert voxels back to points. std::vector<Eigen::Vector3i> voxels(bins_all.begin(), bins_all.end()); Eigen::Matrix3Xd shadow_out = shadowVoxelsToPoints(voxels, voxel_grid_size); return shadow_out; }
int input_run(int id) { int res, i; global->in[id].buf = malloc(256 * 1024); if(global->in[id].buf == NULL) { IPRINT(INPUT_PLUGIN_NAME " - could not allocate memory\n"); exit(EXIT_FAILURE); } plugin_id = id; // auto-detect algorithm CameraAbilitiesList* al; GPPortInfoList* il; CameraList* list; const char* model; const char* port; context = gp_context_new(); gp_abilities_list_new(&al); gp_abilities_list_load(al, context); gp_port_info_list_new(&il); gp_port_info_list_load(il); gp_list_new(&list); gp_abilities_list_detect(al, il, list, context); int count = gp_list_count(list); IPRINT(INPUT_PLUGIN_NAME " - Detected %d camera(s)\n", count); if(count == 0) { IPRINT(INPUT_PLUGIN_NAME " - No cameras detected.\n"); return 0; } GPPortInfo info; CameraAbilities a; int m, p; camera = NULL; for(i = 0; i < count; i++) { res = gp_list_get_name(list, i, &model); CAMERA_CHECK_GP(res, "gp_list_get_name"); m = gp_abilities_list_lookup_model(al, model); if(m < 0) { IPRINT(INPUT_PLUGIN_NAME " - Gphoto abilities_list_lookup_model Code: %d - %s\n", m, gp_result_as_string(m)); return 0; } res = gp_abilities_list_get_abilities(al, m, &a); CAMERA_CHECK_GP(res, "gp_abilities_list_get_abilities"); res = gp_list_get_value(list, i, &port); CAMERA_CHECK_GP(res, "gp_list_get_value"); DBG("Model: %s; port: %s.\n", model, port); if(selected_port != NULL && strcmp(selected_port, port) != 0) continue; p = gp_port_info_list_lookup_path(il, port); if(p < 0) { IPRINT(INPUT_PLUGIN_NAME " - Gphoto port_info_list_lookup_path Code: %d - %s\n", m, gp_result_as_string(m)); return 0; } res = gp_port_info_list_get_info(il, p, &info); CAMERA_CHECK_GP(res, "gp_port_info_list_get_info"); res = gp_camera_new(&camera); CAMERA_CHECK_GP(res, "gp_camera_new"); res = gp_camera_set_abilities(camera, a); CAMERA_CHECK_GP(res, "gp_camera_set_abilities"); res = gp_camera_set_port_info(camera, info); CAMERA_CHECK_GP(res, "gp_camera_set_port_info"); } if(camera == NULL) { IPRINT("Camera %s not found, exiting.\n", selected_port); exit(EXIT_FAILURE); } // cleanup gp_list_unref(list); gp_port_info_list_free(il); gp_abilities_list_free(al); // open camera and set capture on int value = 1; res = gp_camera_init(camera, context); CAMERA_CHECK_GP(res, "gp_camera_init"); camera_set("capture", &value); // starting thread if(pthread_create(&thread, 0, capture, NULL) != 0) { free(global->in[id].buf); IPRINT("could not start worker thread\n"); exit(EXIT_FAILURE); } pthread_detach(thread); return 0; }