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;
}
