int
main (int argc, char *argv[])
{
getopt_t *gopt = getopt_create ();
getopt_add_bool (gopt, 'h', "help", 0, "Show this help screen");
getopt_add_string (gopt, 'd', "device", "/dev/ttyUSB0", "Device name");
getopt_add_int (gopt, 'b', "baud", "1000000", "Device baud rate");
getopt_add_int (gopt, 'n', "num_servos", "6", "Number of servos");
getopt_add_string (gopt, '\0', "status-channel", "ARM_STATUS2", "LCM status channel");
getopt_add_string (gopt, '\0', "command-channel", "ARM_COMMAND2", "LCM command channel");
if (!getopt_parse (gopt, argc, argv, 1) || getopt_get_bool (gopt, "help")) {
getopt_do_usage (gopt);
exit (-1);
}
arm_state_t *arm_state = arm_state_create (getopt_get_string (gopt, "device"),
getopt_get_int (gopt, "baud"),
getopt_get_int (gopt, "num_servos"));
// LCM Initialization
arm_state->lcm = lcm_create (NULL);
arm_state->command_channel = getopt_get_string (gopt, "command-channel");
arm_state->status_channel = getopt_get_string (gopt, "status-channel");
if (!arm_state->lcm)
return -1;
dynamixel_command_list_t_subscribe (arm_state->lcm,
arm_state->command_channel,
command_handler,
arm_state);
pthread_create (&arm_state->status_thread, NULL, status_loop, arm_state);
pthread_create (&arm_state->driver_thread, NULL, driver_loop, arm_state);
// Probably not needed, given how this operates
pthread_join (arm_state->status_thread, NULL);
pthread_join (arm_state->driver_thread, NULL);
// Cleanup
arm_state_destroy (arm_state);
getopt_destroy (gopt);
}
void bt_history(t_cmd *cmd, struct s_env *env)
{
t_app *const app = app_instance();
t_getopt go;
if (cmd->argc == 1)
{
print_history();
return ;
}
getopt_init_args(&go, cmd->argc, cmd->args);
if (getopt_contains(&go, "-c"))
histo_clear(&app->histo);
else if (getopt_contains(&go, "-d"))
remove_item_history(cmd);
else if (getopt_contains(&go, "-r"))
reload_history();
getopt_destroy(&go);
(void)env;
}
int main(int argc, char** argv)
{
// camera
CameraHandler camera;
// getopt
getopt_t* gopt = getopt_create();
getopt_add_string(gopt, 'f', "file", "", "Use static camera image");
if (!getopt_parse(gopt, argc, argv, 1)) {
getopt_do_usage(gopt);
exit(1);
}
const char* fileName = getopt_get_string(gopt, "file");
if (strncmp(fileName, "", 1)) {
// if fileName is not empty
camera.setStaticImage(fileName);
}
getopt_destroy(gopt);
// initialize with first image
image_u32_t* tempIm = camera.getImage();
CalibrationHandler::instance()->calibrateImageSize(tempIm->height, tempIm->width, true);
image_u32_destroy(tempIm);
// lcm
LcmHandler::instance()->launchThreads();
// vx
VxHandler vx(1024, 768);
vx.launchThreads();
while (1) {
CalibrationInfo calibrationInfo =
CalibrationHandler::instance()->getCalibration();
RenderInfo render;
render.im = camera.getImage();
CalibrationHandler::instance()->clipImage(render.im);
// std::array<float, 2> pos;
// if (Arm::instance()->forwardKinematics(pos)) {
// printf("pos: %f, %f\n", pos[0], pos[1]);
// }
if (buttonStates.blobDetect) {
std::vector<BlobDetector::Blob> blobs =
BlobDetector::findBlobs(render.im, calibrationInfo, blobMinPixels);
for (const auto& blob : blobs) {
std::array<int, 2> imageCoords{{blob.x, blob.y}};
std::array<float, 2> screenCoords =
CoordinateConverter::imageToScreen(imageCoords);
switch (blob.type) {
case REDBALL:
render.redBlobs.push_back(screenCoords);
break;
case GREENBALL:
render.greenBlobs.push_back(screenCoords);
break;
case BLUESQUARE:
render.blueBlobs.push_back(screenCoords);
break;
default:
break;
}
}
}
VxButtonStates buttonStates = vx.getButtonStates();
if (buttonStates.colorMask) {
maskWithColors(render.im, calibrationInfo);
}
vx.changeRenderInfo(render);
usleep(1e3);
}
}
int main(int argc, char ** argv)
{
eecs467_init(argc, argv);
state_t * state = (state_t*) calloc(1, sizeof(state_t));
global_state = state;
state->gopt = getopt_create();
state->app.display_finished = display_finished;
state->app.display_started = display_started;
state->app.impl = state;
state->update_arm_cont = 0;
state->update_arm = 0;
state->arm = new RexArm();
state->body = new Body();
state->ds = new DataSmoother(0.4, 0.3, 0, 0);
state->running = 1;
state->set_cbs = 0;
state->controlBoxColor[GRIPPER] = vx_green;
state->controlBoxColor[WRIST] = vx_yellow;
state->controlBoxColor[ARM] = vx_orange;
state->controlBoxColor[ROTATE] = vx_red;
for (int i = 0; i < NUM_CONTROL_BOXES; i++) {
state->controlBoxes[i] = new BoundingBox();
state->controlBoxes[i]->setDimensions(CB_WIDTH, CB_HEIGHT, CB_DEPTH);
}
lcm_t * lcm = lcm_create (NULL);
state->lcm = lcm;
BoundingBox floorBoard, base;
floorBoard.setPosition(0, 0, 0);
floorBoard.setDimensions(100, 100, 10);
state->cfs.addBoundingBox(&floorBoard);
base.setPosition(0, 0, 4);
base.setDimensions(7, 7, 8);
state->cfs.addBoundingBox(&base);
//signal(SIGINT, terminal_signal_handler);
pthread_mutex_init(&state->layer_mutex, NULL);
pthread_mutex_init(&state->lcm_mutex, NULL);
pthread_mutex_init(&state->running_mutex, NULL);
pthread_mutex_init(&state->fsm_mutex, NULL);
state->layer_map = zhash_create(sizeof(vx_display_t*), sizeof(vx_layer_t*), zhash_uint64_hash, zhash_uint64_equals);
getopt_add_bool(state->gopt, 'h', "help", 0, "Show this help");
//getopt_add_bool(state->gopt, 'v', "verbose", 0, "Show extra debugging output");
//getopt_add_int (state->gopt, 'l', "limitKBs", "-1", "Remote display bandwidth limit. < 0: unlimited.");
//getopt_add_double (state->gopt, 'd', "decimate", "0", "Decimate image by this amount before showing in vx");
if (!getopt_parse(state->gopt, argc, argv, 0) ||
getopt_get_bool(state->gopt,"help")) {
getopt_do_usage(state->gopt);
exit(-1);
}
pthread_create(&state->lcm_handle_thread, NULL, lcm_handle_loop, state);
//pthread_create(&state->gui_thread, NULL, gui_create, state);
pthread_create(&state->arm_commander_thread, NULL, arm_commander, state);
pthread_create(&state->fsm_thread, NULL, FSM, state);
//pthread_join(state->gui_thread, NULL);
gui_create(state);
printf("after gui_create\n");
// clean up
delete state->arm;
delete state->body;
delete state->ds;
for (int i = 0; i < NUM_CONTROL_BOXES; i++) {
delete state->controlBoxes[i];
}
vx_global_destroy();
getopt_destroy(state->gopt);
printf("Exited Cleanly!\n");
return 0;
}
int main(int argc, char ** argv)
{
getopt_t *gopt = getopt_create();
getopt_add_bool (gopt, 'h', "help", 0, "Show help");
getopt_add_bool (gopt, '\0', "no-gtk", 0, "Don't show gtk window, only advertise remote connection");
getopt_add_string (gopt, '\0', "pnm", "", "Path for pnm file to render as texture (.e.g BlockM.pnm)");
getopt_add_bool (gopt, '\0', "stay-open", 0, "Stay open after gtk exits to continue handling remote connections");
// parse and print help
if (!getopt_parse(gopt, argc, argv, 1) || getopt_get_bool(gopt,"help")) {
printf ("Usage: %s [options]\n\n", argv[0]);
getopt_do_usage (gopt);
exit (1);
}
vx_global_init(); // Call this to initialize the vx-wide lock. Required to start the GL thread or to use the program library
state_t * state = state_create();
// Load a pnm from file, and repack the data so that it's understandable by vx
if (strcmp(getopt_get_string(gopt,"pnm"),"")) {
state->img = image_u32_create_from_pnm(getopt_get_string(gopt, "pnm"));
printf("Loaded image %d x %d from %s\n",
state->img->width, state->img->height,
getopt_get_string(gopt, "pnm"));
}
draw(state, state->world);
vx_remote_display_source_attr_t remote_attr;
vx_remote_display_source_attr_init(&remote_attr);
remote_attr.advertise_name = "Vx Demo";
vx_remote_display_source_t * cxn = vx_remote_display_source_create_attr(&state->app, &remote_attr);
pthread_create(&state->animate_thread, NULL, render_loop, state);
if (!getopt_get_bool(gopt,"no-gtk")) {
gdk_threads_init ();
gdk_threads_enter ();
gtk_init (&argc, &argv);
vx_gtk_display_source_t * appwrap = vx_gtk_display_source_create(&state->app);
GtkWidget * window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
GtkWidget * canvas = vx_gtk_display_source_get_widget(appwrap);
gtk_window_set_default_size (GTK_WINDOW (window), 400, 400);
gtk_container_add(GTK_CONTAINER(window), canvas);
gtk_widget_show (window);
gtk_widget_show (canvas); // XXX Show all causes errors!
g_signal_connect_swapped(G_OBJECT(window), "destroy", G_CALLBACK(gtk_main_quit), NULL);
gtk_main (); // Blocks as long as GTK window is open
gdk_threads_leave ();
vx_gtk_display_source_destroy(appwrap);
// quit when gtk closes? Or wait for remote displays/Ctrl-C
if (!getopt_get_bool(gopt, "stay-open"))
state->running = 0;
}
pthread_join(state->animate_thread, NULL);
vx_remote_display_source_destroy(cxn);
state_destroy(state);
vx_global_destroy();
getopt_destroy(gopt);
}
int main(int argc, char ** argv)
{
getopt_t *gopt = getopt_create();
getopt_add_bool (gopt, 'h', "help", 0, "Show help");
getopt_add_bool (gopt, '\0', "no-gtk", 0, "Don't show gtk window, only advertise remote connection");
getopt_add_int (gopt, 'l', "limitKBs", "-1", "Remote display bandwidth limit. < 0: unlimited.");
getopt_add_string (gopt, '\0', "pnm", "", "Path for pnm file to render as texture (.e.g BlockM.pnm)");
getopt_add_bool (gopt, '\0', "stay-open", 0, "Stay open after gtk exits to continue handling remote connections");
// parse and print help
if (!getopt_parse(gopt, argc, argv, 1) || getopt_get_bool(gopt,"help")) {
printf ("Usage: %s [options]\n\n", argv[0]);
getopt_do_usage (gopt);
exit (1);
}
signal(SIGPIPE, SIG_IGN); // potential fix for Valgrind "Killed" on
// remote viewer exit
state_t * state = state_create();
// Load a pnm from file, and repack the data so that it's understandable by vx
if (strcmp(getopt_get_string(gopt,"pnm"),"")) {
image_u8_t * img2 = image_u8_create_from_pnm(getopt_get_string(gopt, "pnm"));
state->img = image_util_convert_rgb_to_rgba (img2);
image_u8_destroy (img2);
}
vx_global_init(); // Call this to initialize the vx-wide lock. Required to start the GL thread or to use the program library
vx_application_t app = {.impl=state, .display_started=display_started, .display_finished=display_finished};
vx_remote_display_source_attr_t remote_attr;
vx_remote_display_source_attr_init(&remote_attr);
remote_attr.max_bandwidth_KBs = getopt_get_int(gopt, "limitKBs");
remote_attr.advertise_name = "Vx Stress Test";
vx_remote_display_source_t * cxn = vx_remote_display_source_create_attr(&app, &remote_attr);
for (int i = 0; i < NRENDER; i++) {
tinfo_t * tinfo = calloc(1,sizeof(tinfo_t));
tinfo->state = state;
tinfo->id = i;
pthread_create(&state->render_threads[i], NULL, render_loop, tinfo);
}
pthread_create(&state->camera_thread, NULL, camera_loop, state);
if (!getopt_get_bool(gopt,"no-gtk")) {
gdk_threads_init ();
gdk_threads_enter ();
gtk_init (&argc, &argv);
vx_gtk_display_source_t * appwrap = vx_gtk_display_source_create(&app);
GtkWidget * window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
GtkWidget * canvas = vx_gtk_display_source_get_widget(appwrap);
gtk_window_set_default_size (GTK_WINDOW (window), 400, 400);
gtk_container_add(GTK_CONTAINER(window), canvas);
gtk_widget_show (window);
gtk_widget_show (canvas); // XXX Show all causes errors!
g_signal_connect_swapped(G_OBJECT(window), "destroy", G_CALLBACK(gtk_main_quit), NULL);
gtk_main (); // Blocks as long as GTK window is open
gdk_threads_leave ();
vx_gtk_display_source_destroy(appwrap);
// quit when gtk closes? Or wait for remote displays/Ctrl-C
if (!getopt_get_bool(gopt, "stay-open"))
state->running = 0;
}
for (int i = 0; i < NRENDER; i++)
pthread_join(state->render_threads[i], NULL);
vx_remote_display_source_destroy(cxn);
state_destroy(state);
vx_global_destroy();
getopt_destroy(gopt);
}