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);
}
int main(int argc, char *argv[])
{
getopt_t *getopt = getopt_create();
getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");
getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");
getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use");
getopt_add_int(getopt, '\0', "border", "1", "Set tag family border size");
getopt_add_int(getopt, 'i', "iters", "1", "Repeat processing on input set this many times");
getopt_add_int(getopt, 't', "threads", "4", "Use this many CPU threads");
getopt_add_double(getopt, 'x', "decimate", "1.0", "Decimate input image by this factor");
getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input");
getopt_add_bool(getopt, '1', "refine-decode", 0, "Spend more time trying to decode tags");
getopt_add_bool(getopt, '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");
if (!getopt_parse(getopt, argc, argv, 1) || getopt_get_bool(getopt, "help")) {
printf("Usage: %s [options] <input files>\n", argv[0]);
getopt_do_usage(getopt);
exit(0);
}
const zarray_t *inputs = getopt_get_extra_args(getopt);
apriltag_family_t *tf = NULL;
const char *famname = getopt_get_string(getopt, "family");
if (!strcmp(famname, "tag36h11"))
tf = tag36h11_create();
else if (!strcmp(famname, "tag36h10"))
tf = tag36h10_create();
else if (!strcmp(famname, "tag36artoolkit"))
tf = tag36artoolkit_create();
else if (!strcmp(famname, "tag25h9"))
tf = tag25h9_create();
else if (!strcmp(famname, "tag25h7"))
tf = tag25h7_create();
else {
printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
exit(-1);
}
tf->black_border = getopt_get_int(getopt, "border");
apriltag_detector_t *td = apriltag_detector_create();
apriltag_detector_add_family(td, tf);
td->quad_decimate = getopt_get_double(getopt, "decimate");
td->quad_sigma = getopt_get_double(getopt, "blur");
td->nthreads = getopt_get_int(getopt, "threads");
td->debug = getopt_get_bool(getopt, "debug");
td->refine_decode = getopt_get_bool(getopt, "refine-decode");
td->refine_pose = getopt_get_bool(getopt, "refine-pose");
int quiet = getopt_get_bool(getopt, "quiet");
int maxiters = getopt_get_int(getopt, "iters");
const int hamm_hist_max = 10;
for (int iter = 0; iter < maxiters; iter++) {
if (maxiters > 1)
printf("iter %d / %d\n", iter + 1, maxiters);
for (int input = 0; input < zarray_size(inputs); input++) {
int hamm_hist[hamm_hist_max];
memset(hamm_hist, 0, sizeof(hamm_hist));
char *path;
zarray_get(inputs, input, &path);
if (!quiet)
printf("loading %s\n", path);
image_u8_t *im = image_u8_create_from_pnm(path);
if (im == NULL) {
printf("couldn't find %s\n", path);
continue;
}
zarray_t *detections = apriltag_detector_detect(td, im);
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
if (!quiet)
printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, goodness %8.3f, margin %8.3f\n",
i, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);
hamm_hist[det->hamming]++;
apriltag_detection_destroy(det);
}
zarray_destroy(detections);
if (!quiet) {
timeprofile_display(td->tp);
printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
}
if (!quiet)
printf("Hamming histogram: ");
for (int i = 0; i < hamm_hist_max; i++)
printf("%5d", hamm_hist[i]);
if (quiet) {
printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
}
printf("\n");
image_u8_destroy(im);
}
}
// don't deallocate contents of inputs; those are the argv
apriltag_detector_destroy(td);
tag36h11_destroy(tf);
return 0;
}
static
#define getopt_std_args int argc, CHAR *const argv[], const CHAR *optstring
int getopt_parse( int mode, getopt_std_args, ... )
{
/* Common core implementation for ALL `getopt' functions.
*/
static int argind = 0;
static int optbase = 0;
static const CHAR *nextchar = NULL;
static int optmark = 0;
if( (optreset |= (optind < 1)) || (optind < optbase) )
{
/* POSIX does not prescribe any definitive mechanism for restarting
* a `getopt' scan, but some applications may require such capability.
* We will support it, by allowing the caller to adjust the value of
* `optind' downwards, (nominally setting it to zero). Since POSIX
* wants `optind' to have an initial value of one, but we want all
* of our internal place holders to be initialised to zero, when we
* are called for the first time, we will handle such a reset by
* adjusting all of the internal place holders to one less than
* the adjusted `optind' value, (but never to less than zero).
*/
if( optreset )
{
/* User has explicitly requested reinitialisation...
* We need to reset `optind' to it's normal initial value of 1,
* to avoid a potential infinitely recursive loop; by doing this
* up front, we also ensure that the remaining place holders
* will be correctly reinitialised to no less than zero.
*/
optind = 1;
/* We also need to clear the `optreset' request...
*/
optreset = 0;
}
/* Now, we may safely reinitialise the internal place holders, to
* one less than `optind', without fear of making them negative.
*/
optmark = optbase = argind = optind - 1;
nextchar = NULL;
}
/* From a POSIX perspective, the following is `undefined behaviour';
* we implement it thus, for compatibility with GNU and BSD getopt.
*/
else if( optind > (argind + 1) )
{
/* Some applications expect to be able to manipulate `optind',
* causing `getopt' to skip over one or more elements of `argv';
* POSIX doesn't require us to support this brain-damaged concept;
* (indeed, POSIX defines no particular behaviour, in the event of
* such usage, so it must be considered a bug for an application
* to rely on any particular outcome); nonetheless, Mac-OS-X and
* BSD actually provide *documented* support for this capability,
* so we ensure that our internal place holders keep track of
* external `optind' increments; (`argind' must lag by one).
*/
argind = optind - 1;
/* When `optind' is misused, in this fashion, we also abandon any
* residual text in the argument we had been parsing; this is done
* without any further processing of such abandoned text, assuming
* that the caller is equipped to handle it appropriately.
*/
nextchar = NULL;
}
if( nextchar && *nextchar )
{
/* we are parsing a standard, or short format, option argument ...
*/
const CHAR *optchar;
if( (optchar = getopt_match( optopt = *nextchar++, optstring )) != NULL )
{
/* we have identified it as valid ...
*/
if( optchar[1] == getopt_takes_argument )
{
/* and determined that it requires an associated argument ...
*/
if( ! *(optarg = (char *)(nextchar)) )
{
/* the argument is NOT attached ...
*/
if( optchar[2] == getopt_takes_argument )
/*
* but this GNU extension marks it as optional,
* so we don't provide one on this occasion.
*/
optarg = NULL;
/* otherwise this option takes a mandatory argument,
* so, provided there is one available ...
*/
else if( (argc - argind) > 1 )
/*
* we take the following command line argument,
* as the appropriate option argument.
*/
optarg = argv[++argind];
/* but if no further argument is available,
* then there is nothing we can do, except for
* issuing the requisite diagnostic message.
*/
else
{
complain( "option requires an argument -- %c", optopt );
return getopt_missing_arg( optstring );
}
}
optind = argind + 1;
nextchar = NULL;
}
else
optarg = NULL;
optind = (nextchar && *nextchar) ? argind : argind + 1;
return optopt;
}
/* if we didn't find a valid match for the specified option character,
* then we fall through to here, so take appropriate diagnostic action.
*/
if( mode == getopt_mode_long_only )
{
complain( "unrecognised option `-%s'", --nextchar );
nextchar = NULL;
optopt = 0;
}
else
complain( "invalid option -- %c", optopt );
optind = (nextchar && *nextchar) ? argind : argind + 1;
return getopt_unknown;
}
if( optmark > optbase )
{
/* This can happen, in GNU parsing mode ONLY, when we have
* skipped over non-option arguments, and found a subsequent
* option argument; in this case we permute the arguments.
*/
int index;
/*
* `optspan' specifies the number of contiguous arguments
* which are spanned by the current option, and so must be
* moved together during permutation.
*/
int optspan = argind - optmark + 1;
/*
* we use `this_arg' to store these temporarily.
*/
CHAR *this_arg[optspan];
/*
* we cannot manipulate `argv' directly, since the `getopt'
* API prototypes it as `read-only'; this cast to `arglist'
* allows us to work around that restriction.
*/
CHAR **arglist = (char **)(argv);
/* save temporary copies of the arguments which are associated
* with the current option ...
*/
for( index = 0; index < optspan; ++index )
this_arg[index] = arglist[optmark + index];
/* move all preceding non-option arguments to the right,
* overwriting these saved arguments, while making space
* to replace them in their permuted location.
*/
for( --optmark; optmark >= optbase; --optmark )
arglist[optmark + optspan] = arglist[optmark];
/* restore the temporarily saved option arguments to
* their permuted location.
*/
for( index = 0; index < optspan; ++index )
arglist[optbase + index] = this_arg[index];
/* adjust `optbase', to account for the relocated option.
*/
optbase += optspan;
}
else
/* no permutation occurred ...
* simply adjust `optbase' for all options parsed so far.
*/
optbase = argind + 1;
/* enter main parsing loop ...
*/
while( argc > ++argind )
{
/* inspect each argument in turn, identifying possible options ...
*/
if( is_switchar( *(nextchar = argv[optmark = argind]) ) && *++nextchar )
{
/* we've found a candidate option argument ... */
if( is_switchar( *nextchar ) )
{
/* it's a double hyphen argument ... */
const CHAR *refchar = nextchar;
if( *++refchar )
{
/* and it looks like a long format option ...
* `getopt_long' mode must be active to accept it as such,
* `getopt_long_only' also qualifies, but we must downgrade
* it to force explicit handling as a long format option.
*/
if( mode >= getopt_mode_long )
{
nextchar = refchar;
mode = getopt_mode_long;
}
}
else
{
/* this is an explicit `--' end of options marker, so wrap up now!
*/
if( optmark > optbase )
{
/* permuting the argument list as necessary ...
* (note use of `this_arg' and `arglist', as above).
*/
CHAR *this_arg = argv[optmark];
CHAR **arglist = (CHAR **)(argv);
/* move all preceding non-option arguments to the right ...
*/
do arglist[optmark] = arglist[optmark - 1];
while( optmark-- > optbase );
/* reinstate the `--' marker, in its permuted location.
*/
arglist[optbase] = this_arg;
}
/* ... before finally bumping `optbase' past the `--' marker,
* and returning the `all done' completion indicator.
*/
optind = ++optbase;
return getopt_all_done;
}
}
else if( mode < getopt_mode_long_only )
{
/* it's not an explicit long option, and `getopt_long_only' isn't active,
* so we must explicitly try to match it as a short option.
*/
mode = getopt_mode_standard;
}
if( mode >= getopt_mode_long )
{
/* the current argument is a long form option, (either explicitly,
* introduced by a double hyphen, or implicitly because we were called
* by `getopt_long_only'); this is where we parse it.
*/
int lookup;
int matched = -1;
/* we need to fetch the `extra' function arguments, which are
* specified for the `getopt_long' APIs.
*/
va_list refptr;
va_start( refptr, optstring );
struct option *longopts = va_arg( refptr, struct option * );
int *optindex = va_arg( refptr, int * );
va_end( refptr );
/* ensuring that `optarg' does not inherit any junk, from parsing
* preceding arguments ...
*/
optarg = NULL;
for( lookup = 0; longopts && longopts[lookup].name; ++lookup )
{
/* scan the list of defined long form options ...
*/
switch( getopt_match_long( nextchar, longopts[lookup].name ) )
{
/* looking for possible matches for the current argument.
*/
case getopt_exact_match:
/*
* when an exact match is found,
* return it immediately, setting `nextchar' to NULL,
* to ensure we don't mistakenly try to match any
* subsequent characters as short form options.
*/
nextchar = NULL;
return getopt_resolved( mode, argc, argv, &argind,
longopts, lookup, optindex, optstring );
case getopt_abbreviated_match:
/*
* but, for a partial (initial substring) match ...
*/
if( matched >= 0 )
{
/* if this is not the first, then we have an ambiguity ...
*/
optopt = 0;
nextchar = NULL;
optind = argind + 1;
complain( "option `%s' is ambiguous", argv[argind] );
return getopt_unknown;
}
/* otherwise just note that we've found a possible match ...
*/
matched = lookup;
}
}
if( matched >= 0 )
{
/* if we get to here, then we found exactly one partial match,
* so return it, as for an exact match.
*/
nextchar = NULL;
return getopt_resolved( mode, argc, argv, &argind,
longopts, matched, optindex, optstring );
}
if( mode < getopt_mode_long_only )
{
/* if here, then we had what SHOULD have been a long form option,
* but it is unmatched; (perversely, `mode == getopt_mode_long_only'
* allows us to still try to match it as a short form option).
*/
optopt = 0;
nextchar = NULL;
optind = argind + 1;
complain( "unrecognised option `%s'", argv[argind] );
return getopt_unknown;
}
}
/* fall through to handle standard short form options...
* when the option argument format is neither explictly identified
* as long, nor implicitly matched as such, and the argument isn't
* just a bare hyphen, (which isn't an option), then we make one
* recursive call to explicitly interpret it as short format.
*/
if( *nextchar )
return getopt_parse( mode, argc, argv, optstring );
}
/* if we get to here, then we've parsed a non-option argument ...
* in GNU compatibility mode, we step over it, so we can permute
* any subsequent option arguments, but ...
*/
if( *optstring == getopt_switchar )
{
/* if `optstring' begins with a `-' character, this special
* GNU specific behaviour requires us to return the non-option
* arguments in strict order, as pseudo-arguments to a special
* option, with return value defined as `getopt_ordered'.
*/
nextchar = NULL;
optind = argind + 1;
optarg = argv[argind];
return getopt_ordered;
}
if( getopt_conventions( *optstring ) & getopt_posixly_correct )
/*
* otherwise ...
* for POSIXLY_CORRECT behaviour, or if `optstring' begins with
* a `+' character, then we break out of the parsing loop, so that
* the scan ends at the current argument, with no permutation.
*/
break;
}
int main(int argc, char *argv[])
{
auto options = std::unique_ptr<getopt_t, void(*)(getopt_t*)> (getopt_create(), getopt_destroy);
// getopt_t *options = getopt_create();
getopt_add_bool(options.get(), 'h', "help", 0, "Show this help");
getopt_add_bool(options.get(), 'd', "debug", 0, "Enable debugging output (slow)");
getopt_add_bool(options.get(), 'w', "window", 1, "Show the detected tags in a window");
getopt_add_bool(options.get(), 'q', "quiet", 0, "Reduce output");
getopt_add_int(options.get(), '\0', "border", "1", "Set tag family border size");
getopt_add_int(options.get(), 't', "threads", "4", "Use this many CPU threads");
getopt_add_double(options.get(), 'x', "decimate", "1.0", "Decimate input image by this factor");
getopt_add_double(options.get(), 'b', "blur", "0.0", "Apply low-pass blur to input");
getopt_add_bool(options.get(), '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
getopt_add_bool(options.get(), '1', "refine-decode", 0, "Spend more time trying to decode tags");
getopt_add_bool(options.get(), '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");
getopt_add_double(options.get(), 's', "size", "0.04047", "Physical side-length of the tag (meters)");
getopt_add_int(options.get(), 'c', "camera", "0", "Camera ID");
getopt_add_int(options.get(), 'i', "tag_id", "-1", "Tag ID (-1 for all tags in family)");
if (!getopt_parse(options.get(), argc, argv, 1) || getopt_get_bool(options.get(), "help")) {
printf("Usage: %s [options]\n", argv[0]);
getopt_do_usage(options.get());
exit(0);
}
AprilTagDetector tag_detector(options);
auto lcm = std::make_shared<lcm::LCM>();
Eigen::Matrix3d camera_matrix = Eigen::Matrix3d::Identity();
// camera_matrix(0,0) = bot_camtrans_get_focal_length_x(mCamTransLeft);
// camera_matrix(1,1) = bot_camtrans_get_focal_length_y(mCamTransLeft);
// camera_matrix(0,2) = bot_camtrans_get_principal_x(mCamTransLeft);
// camera_matrix(1,2) = bot_camtrans_get_principal_y(mCamTransLeft);
camera_matrix(0,0) = 535.04778754;
camera_matrix(1,1) = 533.37100256;
camera_matrix(0,2) = 302.83654976;
camera_matrix(1,2) = 237.69023961;
Eigen::Vector4d distortion_coefficients(-7.74010810e-02, -1.97835565e-01, -4.47956948e-03, -5.42361499e-04);
// camera matrix:
// [[ 535.04778754 0. 302.83654976]
// [ 0. 533.37100256 237.69023961]
// [ 0. 0. 1. ]]
// distortion coefficients: [ -7.74010810e-02 -1.97835565e-01 -4.47956948e-03 -5.42361499e-04
// 9.30985112e-01]
cv::VideoCapture capture(getopt_get_int(options.get(), "camera"));
if (!capture.isOpened()) {
std::cout << "Cannot open the video cam" << std::endl;
return -1;
}
cv::Mat frame;
Eigen::Isometry3d tag_to_camera = Eigen::Isometry3d::Identity();
crazyflie_t::webcam_pos_t tag_to_camera_msg;
while (capture.read(frame)) {
std::vector<TagMatch> tags = tag_detector.detectTags(frame);
if (tags.size() > 0) {
tag_to_camera = getRelativeTransform(tags[0], camera_matrix, distortion_coefficients, tag_detector.getTagSize());
tag_to_camera_msg = encodeWebcamPos(tag_to_camera);
tag_to_camera_msg.frame_id = 1;
} else {
tag_to_camera_msg = encodeWebcamPos(tag_to_camera);
tag_to_camera_msg.frame_id = -1;
}
tag_to_camera_msg.timestamp = timestamp_now();
lcm->publish("WEBCAM_POS", &tag_to_camera_msg);
}
return 0;
}
int main(int argc, char *argv[])
{
getopt_t *gopt = getopt_create();
getopt_add_bool (gopt, 'h', "help", 0, "Show this help");
getopt_add_bool (gopt, 't', "tokenize", 0, "Show tokenization");
getopt_add_bool (gopt, 'd', "debug", 0, "Show parsed file");
getopt_add_bool (gopt, 0, "lazy", 0, "Generate output file only if .lcm is newer");
getopt_add_string(gopt, 0, "package-prefix", "",
"Add this package name as a prefix to the declared package");
getopt_add_bool (gopt, 0, "version", 0, "Show version information and exit");
// we only support portable declarations now.
// getopt_add_bool (gopt, 0, "warn-unsafe", 1, "Warn about unportable declarations");
getopt_add_spacer(gopt, "**** C options ****");
getopt_add_bool (gopt, 'c', "c", 0, "Emit C code");
setup_c_options(gopt);
getopt_add_spacer(gopt, "**** C++ options ****");
getopt_add_bool (gopt, 'x', "cpp", 0, "Emit C++ code");
setup_cpp_options(gopt);
getopt_add_spacer(gopt, "**** Java options ****");
getopt_add_bool (gopt, 'j', "java", 0, "Emit Java code");
setup_java_options(gopt);
getopt_add_spacer(gopt, "**** Python options ****");
getopt_add_bool (gopt, 'p', "python", 0, "Emit Python code");
setup_python_options(gopt);
getopt_add_spacer(gopt, "**** Lua options ****");
getopt_add_bool (gopt, 'l', "lua", 0, "Emit Lua code");
setup_lua_options(gopt);
getopt_add_spacer(gopt, "**** C#.NET options ****");
getopt_add_bool (gopt, 0, "csharp", 0, "Emit C#.NET code");
setup_csharp_options(gopt);
if (!getopt_parse(gopt, argc, argv, 1) || getopt_get_bool(gopt,"help")) {
printf("Usage: %s [options] <input files>\n\n", argv[0]);
getopt_do_usage(gopt);
return 0;
}
lcmgen_t *lcm = lcmgen_create();
lcm->gopt = gopt;
for (unsigned int i = 0; i < g_ptr_array_size(gopt->extraargs); i++) {
char *path = (char *) g_ptr_array_index(gopt->extraargs, i);
int res = lcmgen_handle_file(lcm, path);
if (res)
return res;
}
// If "--version" was specified, then show version information and exit.
if (getopt_get_bool(gopt, "version")) {
printf("lcm-gen %d.%d.%d\n", LCM_MAJOR_VERSION, LCM_MINOR_VERSION,
LCM_MICRO_VERSION);
return 0;
}
// If "-t" or "--tokenize" was specified, then show tokenization
// information and exit.
if (getopt_get_bool(gopt, "tokenize")) {
return 0;
}
int did_something = 0;
if (getopt_get_bool(gopt, "debug")) {
did_something = 1;
lcmgen_dump(lcm);
}
if (getopt_get_bool(gopt, "c")) {
did_something = 1;
if (emit_c(lcm)) {
printf("An error occurred while emitting C code.\n");
}
}
if (getopt_get_bool(gopt, "cpp")) {
did_something = 1;
if (emit_cpp(lcm)) {
printf("An error occurred while emitting C++ code.\n");
}
}
if (getopt_get_bool(gopt, "java")) {
did_something = 1;
if (emit_java(lcm)) {
perror("An error occurred while emitting Java code.\n");
}
}
if (getopt_get_bool(gopt, "python")) {
did_something = 1;
if (emit_python(lcm)) {
printf("An error occurred while emitting Python code.\n");
}
}
if (getopt_get_bool(gopt, "lua")) {
did_something = 1;
if (emit_lua(lcm)) {
printf("An error occurred while emitting Lua code.\n");
}
}
if (getopt_get_bool(gopt, "csharp")) {
did_something = 1;
if (emit_csharp(lcm)) {
printf("An error occurred while emitting C#.NET code.\n");
}
}
if (did_something == 0) {
printf("No actions specified. Try --help.\n");
}
return 0;
}
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[])
{
getopt_t *gopt = getopt_create();
getopt_add_bool (gopt, 'h', "help", 0, "Show this help");
getopt_add_bool (gopt, 't', "tokenize", 0, "Show tokenization");
getopt_add_bool (gopt, 'd', "debug", 0, "Show parsed file");
getopt_add_bool (gopt, 0, "lazy", 0, "Generate output file only if .lcm is newer");
// we only support portable declarations now.
// getopt_add_bool (gopt, 0, "warn-unsafe", 1, "Warn about unportable declarations");
getopt_add_spacer(gopt, "**** C options ****");
getopt_add_bool (gopt, 'c', "c", 0, "Emit C code");
setup_c_options(gopt);
getopt_add_spacer(gopt, "**** C++ options ****");
getopt_add_bool (gopt, 'x', "cpp", 0, "Emit C++ code");
setup_cpp_options(gopt);
getopt_add_spacer(gopt, "**** Java options ****");
getopt_add_bool (gopt, 'j', "java", 0, "Emit Java code");
setup_java_options(gopt);
getopt_add_spacer(gopt, "**** Python options ****");
getopt_add_bool (gopt, 'p', "python", 0, "Emit Python code");
setup_python_options(gopt);
getopt_add_spacer(gopt, "**** Lua options ****");
getopt_add_bool (gopt, 'l', "lua", 0, "Emit Lua code");
setup_lua_options(gopt);
getopt_add_spacer(gopt, "**** C#.NET options ****");
getopt_add_bool (gopt, 0, "csharp", 0, "Emit C#.NET code");
setup_csharp_options(gopt);
if (!getopt_parse(gopt, argc, argv, 1) || getopt_get_bool(gopt,"help")) {
printf("Usage: %s [options] <input files>\n\n", argv[0]);
getopt_do_usage(gopt);
return 0;
}
lcmgen_t *lcm = lcmgen_create();
lcm->gopt = gopt;
for (unsigned int i = 0; i < g_ptr_array_size(gopt->extraargs); i++) {
char *path = (char *) g_ptr_array_index(gopt->extraargs, i);
int res = lcmgen_handle_file(lcm, path);
if (res)
return -1;
}
int did_something = 0;
// if they requested tokenizing (debug) output, we've done that now. Exit.
if (getopt_get_bool(gopt, "tokenize")) {
did_something = 1;
return 0;
}
if (getopt_get_bool(gopt, "debug")) {
did_something = 1;
lcmgen_dump(lcm);
}
if (getopt_get_bool(gopt, "c")) {
did_something = 1;
if (emit_c(lcm)) {
printf("An error occurred while emitting C code.\n");
}
}
if (getopt_get_bool(gopt, "cpp")) {
did_something = 1;
if (emit_cpp(lcm)) {
printf("An error occurred while emitting C++ code.\n");
}
}
if (getopt_get_bool(gopt, "java")) {
did_something = 1;
if (emit_java(lcm)) {
perror("An error occurred while emitting Java code.\n");
}
}
if (getopt_get_bool(gopt, "python")) {
did_something = 1;
if (emit_python(lcm)) {
printf("An error occurred while emitting Python code.\n");
}
}
if (getopt_get_bool(gopt, "lua")) {
did_something = 1;
if (emit_lua(lcm)) {
printf("An error occurred while emitting Lua code.\n");
}
}
if (getopt_get_bool(gopt, "csharp")) {
did_something = 1;
if (emit_csharp(lcm)) {
printf("An error occurred while emitting C#.NET code.\n");
}
}
if (did_something == 0) {
printf("No actions specified. Try --help.\n");
}
return 0;
}
int
main (int argc, char *argv[])
{
// so that redirected stdout won't be insanely buffered.
setvbuf (stdout, (char *) NULL, _IONBF, 0);
state_t *state = calloc (1, sizeof *state);
state->meters_per_tick = METERS_PER_TICK; // IMPLEMENT ME
state->alpha = ALPHA;
state->beta = BETA;
// file for gyro integration test
//state->fp = fopen("gyro-yaw-integration.txt","w");
state->fp = fopen("yaw.txt","w");
// Tests for the position estimate
/*
update_position(state,4456,4456);
update_position(state,0,1208);
update_position(state,4456,4456);
while(1);
*/
// Used for testing the gyro bias
/*
for(int i=0; i<100; i++){
state->yaw_cal_array[i] = 15*i;
}
state->gyro_bias = find_gyro_bias(state);
printf("Gyro Bias: %llu\n", state->gyro_bias);
*/
//printf("getting options\n");
state->gopt = getopt_create ();
getopt_add_bool (state->gopt, 'h', "help", 0, "Show help");
getopt_add_bool (state->gopt, 'g', "use-gyro", 0, "Use gyro for heading instead of wheel encoders");
getopt_add_string (state->gopt, '\0', "odometry-channel", "BOTLAB_ODOMETRY", "LCM channel name");
getopt_add_string (state->gopt, '\0', "feedback-channel", "MAEBOT_MOTOR_FEEDBACK", "LCM channel name");
getopt_add_string (state->gopt, '\0', "sensor-channel", "MAEBOT_SENSOR_DATA", "LCM channel name");
getopt_add_double (state->gopt, '\0', "alpha", ALPHA_STRING, "Longitudinal covariance scaling factor");
getopt_add_double (state->gopt, '\0', "beta", BETA_STRING, "Lateral side-slip covariance scaling factor");
getopt_add_double (state->gopt, '\0', "gyro-rms", GYRO_RMS_STRING, "Gyro RMS deg/s");
if (!getopt_parse (state->gopt, argc, argv, 1) || getopt_get_bool (state->gopt, "help")) {
printf ("Usage: %s [--url=CAMERAURL] [other options]\n\n", argv[0]);
getopt_do_usage (state->gopt);
exit (EXIT_FAILURE);
}
state->use_gyro = getopt_get_bool (state->gopt, "use-gyro");
state->odometry_channel = getopt_get_string (state->gopt, "odometry-channel");
state->feedback_channel = getopt_get_string (state->gopt, "feedback-channel");
state->sensor_channel = getopt_get_string (state->gopt, "sensor-channel");
state->alpha = getopt_get_double (state->gopt, "alpha");
state->beta = getopt_get_double (state->gopt, "beta");
state->gyro_rms = getopt_get_double (state->gopt, "gyro-rms") * DTOR;
state->yaw_calibrated = 0;
state->yaw = 0;
state->yaw_old = 0;
//printf("subscribing to channels\n");
// initialize LCM
state->lcm = lcm_create (NULL);
maebot_motor_feedback_t_subscribe (state->lcm, state->feedback_channel,
motor_feedback_handler, state);
maebot_sensor_data_t_subscribe (state->lcm, state->sensor_channel,
sensor_data_handler, state);
printf ("ticks per meter: %f\n", 1.0/state->meters_per_tick);
while (1){
lcm_handle (state->lcm);
}
}
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);
}
int main(int argc, char *argv[])
{
g_type_init ();
dbg_init ();
// initialize application state and zero out memory
g_self = (state_t*) calloc( 1, sizeof(state_t) );
state_t *self = g_self;
// run the main loop
self->loop = g_main_loop_new(NULL, FALSE);
getopt_t *gopt = getopt_create();
getopt_add_bool (gopt, 'h', "help", 0, "Show this help");
getopt_add_bool (gopt, 'v', "verbose", 0, "Be verbose");
if (!getopt_parse(gopt, argc, argv, 1) || getopt_get_bool(gopt,"help")) {
printf("Usage: %s [options]\n\n", argv[0]);
getopt_do_usage(gopt);
return 0;
}
self->verbose = getopt_get_bool(gopt, "verbose");
self->lcm = lcm_create(NULL);
if (!self->lcm)
return 1;
// read config file
if (!(self->config = read_config_file ())) {
dbg (DBG_ERROR, "[viewer] failed to read config file.");
return -1;
}
// attach lcm to main loop
glib_mainloop_attach_lcm (self->lcm);
// publish cam settings every now and then
// g_timeout_add_seconds (2, &publish_log_info_data, self);
// g_timeout_add_seconds (2, &dump_to_index_file, self);
// listen to tablet event
navlcm_log_info_t_subscribe (self->lcm, "LOG_INFO", on_log_info_set_event, self);
list_files (self);
publish_log_info_data (self);
// connect to kill signal
signal_pipe_glib_quit_on_kill (self->loop);
// run main loop
g_main_loop_run (self->loop);
// cleanup
g_main_loop_unref (self->loop);
return 0;
}
