Squashed 'third_party/apriltag/' content from commit 3e8e974d0
git-subtree-dir: third_party/apriltag
git-subtree-split: 3e8e974d0d8d6ab318abf56d87506d15d7f2cc35
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I04ba3cb2106b6813a1013d57aa8074c26f856598
diff --git a/common/getopt.c b/common/getopt.c
new file mode 100644
index 0000000..e645089
--- /dev/null
+++ b/common/getopt.c
@@ -0,0 +1,548 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson@umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+#include <ctype.h>
+#include <errno.h>
+
+#include "zhash.h"
+#include "zarray.h"
+#include "getopt.h"
+#include "common/math_util.h"
+
+#define GOO_BOOL_TYPE 1
+#define GOO_STRING_TYPE 2
+
+typedef struct getopt_option getopt_option_t;
+
+struct getopt_option
+{
+ char *sname;
+ char *lname;
+ char *svalue;
+
+ char *help;
+ int type;
+
+ int spacer;
+
+ int was_specified;
+};
+
+struct getopt
+{
+ zhash_t *lopts;
+ zhash_t *sopts;
+ zarray_t *extraargs;
+ zarray_t *options;
+};
+
+getopt_t *getopt_create()
+{
+ getopt_t *gopt = (getopt_t*) calloc(1, sizeof(getopt_t));
+
+ gopt->lopts = zhash_create(sizeof(char*), sizeof(getopt_option_t*), zhash_str_hash, zhash_str_equals);
+ gopt->sopts = zhash_create(sizeof(char*), sizeof(getopt_option_t*), zhash_str_hash, zhash_str_equals);
+ gopt->options = zarray_create(sizeof(getopt_option_t*));
+ gopt->extraargs = zarray_create(sizeof(char*));
+
+ return gopt;
+}
+
+void getopt_option_destroy(getopt_option_t *goo)
+{
+ free(goo->sname);
+ free(goo->lname);
+ free(goo->svalue);
+ free(goo->help);
+ memset(goo, 0, sizeof(getopt_option_t));
+ free(goo);
+}
+
+void getopt_destroy(getopt_t *gopt)
+{
+ // free the extra arguments and container
+ zarray_vmap(gopt->extraargs, free);
+ zarray_destroy(gopt->extraargs);
+
+ // deep free of the getopt_option structs. Also frees key/values, so
+ // after this loop, hash tables will no longer work
+ zarray_vmap(gopt->options, getopt_option_destroy);
+ zarray_destroy(gopt->options);
+
+ // free tables
+ zhash_destroy(gopt->lopts);
+ zhash_destroy(gopt->sopts);
+
+ memset(gopt, 0, sizeof(getopt_t));
+ free(gopt);
+}
+
+static void getopt_modify_string(char **str, char *newvalue)
+{
+ char *old = *str;
+ *str = newvalue;
+ if (old != NULL)
+ free(old);
+}
+
+static char *get_arg_assignment(char *arg)
+{
+ // not an arg starting with "--"?
+ if (!str_starts_with(arg, "--")) {
+ return NULL;
+ }
+
+ int eq_index = str_indexof(arg, "=");
+
+ // no assignment?
+ if (eq_index == -1) {
+ return NULL;
+ }
+
+ // no quotes allowed before '=' in "--key=value" option specification.
+ // quotes can be used in value string, or by extra arguments
+ for (int i = 0; i < eq_index; i++) {
+ if (arg[i] == '\'' || arg[i] == '"') {
+ return NULL;
+ }
+ }
+
+ return &arg[eq_index];
+}
+
+// returns 1 if no error
+int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors)
+{
+ int okay = 1;
+ zarray_t *toks = zarray_create(sizeof(char*));
+
+ // take the input stream and chop it up into tokens
+ for (int i = 1; i < argc; i++) {
+
+ char *arg = strdup(argv[i]);
+ char *eq = get_arg_assignment(arg);
+
+ // no equal sign? Push the whole thing.
+ if (eq == NULL) {
+ zarray_add(toks, &arg);
+ } else {
+ // there was an equal sign. Push the part
+ // before and after the equal sign
+ char *val = strdup(&eq[1]);
+ eq[0] = 0;
+ zarray_add(toks, &arg);
+
+ // if the part after the equal sign is
+ // enclosed by quotation marks, strip them.
+ if (val[0]=='\"') {
+ size_t last = strlen(val) - 1;
+ if (val[last]=='\"')
+ val[last] = 0;
+ char *valclean = strdup(&val[1]);
+ zarray_add(toks, &valclean);
+ free(val);
+ } else {
+ zarray_add(toks, &val);
+ }
+ }
+ }
+
+ // now loop over the elements and evaluate the arguments
+ unsigned int i = 0;
+
+ char *tok = NULL;
+
+ while (i < zarray_size(toks)) {
+
+ // rather than free statement throughout this while loop
+ if (tok != NULL)
+ free(tok);
+
+ zarray_get(toks, i, &tok);
+
+ if (!strncmp(tok,"--", 2)) {
+ char *optname = &tok[2];
+ getopt_option_t *goo = NULL;
+ zhash_get(gopt->lopts, &optname, &goo);
+ if (goo == NULL) {
+ okay = 0;
+ if (showErrors)
+ printf("Unknown option --%s\n", optname);
+ i++;
+ continue;
+ }
+
+ goo->was_specified = 1;
+
+ if (goo->type == GOO_BOOL_TYPE) {
+ if ((i+1) < zarray_size(toks)) {
+ char *val = NULL;
+ zarray_get(toks, i+1, &val);
+
+ if (!strcmp(val,"true")) {
+ i+=2;
+ getopt_modify_string(&goo->svalue, val);
+ continue;
+ }
+ if (!strcmp(val,"false")) {
+ i+=2;
+ getopt_modify_string(&goo->svalue, val);
+ continue;
+ }
+ }
+ getopt_modify_string(&goo->svalue, strdup("true"));
+ i++;
+ continue;
+ }
+
+ if (goo->type == GOO_STRING_TYPE) {
+ // TODO: check whether next argument is an option, denoting missing argument
+ if ((i+1) < zarray_size(toks)) {
+ char *val = NULL;
+ zarray_get(toks, i+1, &val);
+ i+=2;
+ getopt_modify_string(&goo->svalue, val);
+ continue;
+ }
+
+ okay = 0;
+ if (showErrors)
+ printf("Option %s requires a string argument.\n",optname);
+ }
+ }
+
+ if (!strncmp(tok,"-",1) && strncmp(tok,"--",2)) {
+ size_t len = strlen(tok);
+ int pos;
+ for (pos = 1; pos < len; pos++) {
+ char sopt[2];
+ sopt[0] = tok[pos];
+ sopt[1] = 0;
+ char *sopt_ptr = (char*) &sopt;
+ getopt_option_t *goo = NULL;
+ zhash_get(gopt->sopts, &sopt_ptr, &goo);
+
+ if (goo==NULL) {
+ // is the argument a numerical literal that happens to be negative?
+ if (pos==1 && isdigit(tok[pos])) {
+ zarray_add(gopt->extraargs, &tok);
+ tok = NULL;
+ break;
+ } else {
+ okay = 0;
+ if (showErrors)
+ printf("Unknown option -%c\n", tok[pos]);
+ i++;
+ continue;
+ }
+ }
+
+ goo->was_specified = 1;
+
+ if (goo->type == GOO_BOOL_TYPE) {
+ getopt_modify_string(&goo->svalue, strdup("true"));
+ continue;
+ }
+
+ if (goo->type == GOO_STRING_TYPE) {
+ if ((i+1) < zarray_size(toks)) {
+ char *val = NULL;
+ zarray_get(toks, i+1, &val);
+ // TODO: allow negative numerical values for short-name options ?
+ if (val[0]=='-')
+ {
+ okay = 0;
+ if (showErrors)
+ printf("Ran out of arguments for option block %s\n", tok);
+ }
+ i++;
+ getopt_modify_string(&goo->svalue, val);
+ continue;
+ }
+
+ okay = 0;
+ if (showErrors)
+ printf("Option -%c requires a string argument.\n", tok[pos]);
+ }
+ }
+ i++;
+ continue;
+ }
+
+ // it's not an option-- it's an argument.
+ zarray_add(gopt->extraargs, &tok);
+ tok = NULL;
+ i++;
+ }
+ if (tok != NULL)
+ free(tok);
+
+ zarray_destroy(toks);
+
+ return okay;
+}
+
+void getopt_add_spacer(getopt_t *gopt, const char *s)
+{
+ getopt_option_t *goo = (getopt_option_t*) calloc(1, sizeof(getopt_option_t));
+ goo->spacer = 1;
+ goo->help = strdup(s);
+ zarray_add(gopt->options, &goo);
+}
+
+void getopt_add_bool(getopt_t *gopt, char sopt, const char *lname, int def, const char *help)
+{
+ char sname[2];
+ sname[0] = sopt;
+ sname[1] = 0;
+ char *sname_ptr = (char*) &sname;
+
+ if (strlen(lname) < 1) { // must have long name
+ fprintf (stderr, "getopt_add_bool(): must supply option name\n");
+ exit (EXIT_FAILURE);
+ }
+
+ if (sopt == '-') { // short name cannot be '-' (no way to reference)
+ fprintf (stderr, "getopt_add_bool(): invalid option character: '%c'\n", sopt);
+ exit (EXIT_FAILURE);
+ }
+
+ if (zhash_contains(gopt->lopts, &lname)) {
+ fprintf (stderr, "getopt_add_bool(): duplicate option name: --%s\n", lname);
+ exit (EXIT_FAILURE);
+ }
+
+ if (sopt != '\0' && zhash_contains(gopt->sopts, &sname_ptr)) {
+ fprintf (stderr, "getopt_add_bool(): duplicate option: -%s ('%s')\n", sname, lname);
+ exit (EXIT_FAILURE);
+ }
+
+ getopt_option_t *goo = (getopt_option_t*) calloc(1, sizeof(getopt_option_t));
+ goo->sname=strdup(sname);
+ goo->lname=strdup(lname);
+ goo->svalue=strdup(def ? "true" : "false");
+ goo->type=GOO_BOOL_TYPE;
+ goo->help=strdup(help);
+
+ zhash_put(gopt->lopts, &goo->lname, &goo, NULL, NULL);
+ zhash_put(gopt->sopts, &goo->sname, &goo, NULL, NULL);
+ zarray_add(gopt->options, &goo);
+}
+
+void getopt_add_int(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help)
+{
+ getopt_add_string(gopt, sopt, lname, def, help);
+}
+
+void
+getopt_add_double (getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help)
+{
+ getopt_add_string (gopt, sopt, lname, def, help);
+}
+
+void getopt_add_string(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help)
+{
+ char sname[2];
+ sname[0] = sopt;
+ sname[1] = 0;
+ char *sname_ptr = (char*) &sname;
+
+ if (strlen(lname) < 1) { // must have long name
+ fprintf (stderr, "getopt_add_string(): must supply option name\n");
+ exit (EXIT_FAILURE);
+ }
+
+ if (sopt == '-') { // short name cannot be '-' (no way to reference)
+ fprintf (stderr, "getopt_add_string(): invalid option character: '%c'\n", sopt);
+ exit (EXIT_FAILURE);
+ }
+
+ if (zhash_contains(gopt->lopts, &lname)) {
+ fprintf (stderr, "getopt_add_string(): duplicate option name: --%s\n", lname);
+ exit (EXIT_FAILURE);
+ }
+
+ if (sopt != '\0' && zhash_contains(gopt->sopts, &sname_ptr)) {
+ fprintf (stderr, "getopt_add_string(): duplicate option: -%s ('%s')\n", sname, lname);
+ exit (EXIT_FAILURE);
+ }
+
+ getopt_option_t *goo = (getopt_option_t*) calloc(1, sizeof(getopt_option_t));
+ goo->sname=strdup(sname);
+ goo->lname=strdup(lname);
+ goo->svalue=strdup(def);
+ goo->type=GOO_STRING_TYPE;
+ goo->help=strdup(help);
+
+ zhash_put(gopt->lopts, &goo->lname, &goo, NULL, NULL);
+ zhash_put(gopt->sopts, &goo->sname, &goo, NULL, NULL);
+ zarray_add(gopt->options, &goo);
+}
+
+const char *getopt_get_string(getopt_t *gopt, const char *lname)
+{
+ getopt_option_t *goo = NULL;
+ zhash_get(gopt->lopts, &lname, &goo);
+ // could return null, but this would be the only
+ // method that doesn't assert on a missing key
+ assert (goo != NULL);
+ return goo->svalue;
+}
+
+int getopt_get_int(getopt_t *getopt, const char *lname)
+{
+ const char *v = getopt_get_string(getopt, lname);
+ assert(v != NULL);
+
+ errno = 0;
+ char *endptr = (char *) v;
+ long val = strtol(v, &endptr, 10);
+
+ if (errno != 0) {
+ fprintf (stderr, "--%s argument: strtol failed: %s\n", lname, strerror(errno));
+ exit (EXIT_FAILURE);
+ }
+
+ if (endptr == v) {
+ fprintf (stderr, "--%s argument cannot be parsed as an int\n", lname);
+ exit (EXIT_FAILURE);
+ }
+
+ return (int) val;
+}
+
+int getopt_get_bool(getopt_t *getopt, const char *lname)
+{
+ const char *v = getopt_get_string(getopt, lname);
+ assert (v!=NULL);
+ int val = !strcmp(v, "true");
+ return val;
+}
+
+double getopt_get_double (getopt_t *getopt, const char *lname)
+{
+ const char *v = getopt_get_string (getopt, lname);
+ assert (v!=NULL);
+
+ errno = 0;
+ char *endptr = (char *) v;
+ double d = strtod (v, &endptr);
+
+ if (errno != 0) {
+ fprintf (stderr, "--%s argument: strtod failed: %s\n", lname, strerror(errno));
+ exit (EXIT_FAILURE);
+ }
+
+ if (endptr == v) {
+ fprintf (stderr, "--%s argument cannot be parsed as a double\n", lname);
+ exit (EXIT_FAILURE);
+ }
+
+ return d;
+}
+
+int getopt_was_specified(getopt_t *getopt, const char *lname)
+{
+ getopt_option_t *goo = NULL;
+ zhash_get(getopt->lopts, &lname, &goo);
+ if (goo == NULL)
+ return 0;
+
+ return goo->was_specified;
+}
+
+const zarray_t *getopt_get_extra_args(getopt_t *gopt)
+{
+ return gopt->extraargs;
+}
+
+void getopt_do_usage(getopt_t * gopt)
+{
+ char * usage = getopt_get_usage(gopt);
+ printf("%s", usage);
+ free(usage);
+}
+
+char * getopt_get_usage(getopt_t *gopt)
+{
+ string_buffer_t * sb = string_buffer_create();
+
+ int leftmargin=2;
+ int longwidth=12;
+ int valuewidth=10;
+
+ for (unsigned int i = 0; i < zarray_size(gopt->options); i++) {
+ getopt_option_t *goo = NULL;
+ zarray_get(gopt->options, i, &goo);
+
+ if (goo->spacer)
+ continue;
+
+ longwidth = max(longwidth, (int) strlen(goo->lname));
+
+ if (goo->type == GOO_STRING_TYPE)
+ valuewidth = max(valuewidth, (int) strlen(goo->svalue));
+ }
+
+ for (unsigned int i = 0; i < zarray_size(gopt->options); i++) {
+ getopt_option_t *goo = NULL;
+ zarray_get(gopt->options, i, &goo);
+
+ if (goo->spacer)
+ {
+ if (goo->help==NULL || strlen(goo->help)==0)
+ string_buffer_appendf(sb,"\n");
+ else
+ string_buffer_appendf(sb,"\n%*s%s\n\n", leftmargin, "", goo->help);
+ continue;
+ }
+
+ string_buffer_appendf(sb,"%*s", leftmargin, "");
+
+ if (goo->sname[0]==0)
+ string_buffer_appendf(sb," ");
+ else
+ string_buffer_appendf(sb,"-%c | ", goo->sname[0]);
+
+ string_buffer_appendf(sb,"--%*s ", -longwidth, goo->lname);
+
+ string_buffer_appendf(sb," [ %s ]", goo->svalue); // XXX: displays current value rather than default value
+
+ string_buffer_appendf(sb,"%*s", (int) (valuewidth-strlen(goo->svalue)), "");
+
+ string_buffer_appendf(sb," %s ", goo->help);
+ string_buffer_appendf(sb,"\n");
+ }
+
+ char * usage = string_buffer_to_string(sb);
+ string_buffer_destroy(sb);
+ return usage;
+}