/** Emit output that is common to every header file **/
static void emit_header_top(lcmgen_t *lcm, FILE *f, char *name)
{
emit_auto_generated_warning(f);
fprintf(f, "#include <stdint.h>\n");
fprintf(f, "#include <stdlib.h>\n");
fprintf(f, "#include <lcm/lcm_coretypes.h>\n");
// fprintf(f, "#include \"%s%slcm_lib.h\"\n",
// getopt_get_string(lcm->gopt, "cinclude"),
// strlen(getopt_get_string(lcm->gopt, "cinclude"))>0 ? "/" : "");
if(!getopt_get_bool(lcm->gopt, "c-no-pubsub")) {
fprintf(f, "#include <lcm/lcm.h>\n");
}
if(strlen(getopt_get_string(lcm->gopt, "c-export-include"))) {
fprintf(f, "#include \"%s%s%s\"\n",
getopt_get_string(lcm->gopt, "cinclude"),
strlen(getopt_get_string(lcm->gopt, "cinclude"))>0 ? "/" : "",
getopt_get_string(lcm->gopt, "c-export-include"));
}
fprintf(f, "\n");
fprintf(f, "#ifndef _%s_h\n", name);
fprintf(f, "#define _%s_h\n", name);
fprintf(f, "\n");
fprintf(f, "#ifdef __cplusplus\n");
fprintf(f, "extern \"C\" {\n");
fprintf(f, "#endif\n");
fprintf(f, "\n");
}
int emit_cpp(lcmgen_t *lcmgen)
{
// iterate through all defined message types
for (unsigned int i = 0; i < g_ptr_array_size(lcmgen->structs); i++) {
lcm_struct_t *lr = (lcm_struct_t *) g_ptr_array_index(lcmgen->structs, i);
const char *tn = lr->structname->lctypename;
char *tn_ = dots_to_slashes(tn);
// compute the target filename
char *header_name = g_strdup_printf("%s%s%s.hpp",
getopt_get_string(lcmgen->gopt, "cpp-hpath"),
strlen(getopt_get_string(lcmgen->gopt, "cpp-hpath")) > 0 ? G_DIR_SEPARATOR_S : "",
tn_);
// generate code if needed
if (lcm_needs_generation(lcmgen, lr->lcmfile, header_name)) {
make_dirs_for_file(header_name);
FILE *f = fopen(header_name, "w");
if (f == NULL)
return -1;
emit_header_start(lcmgen, f, lr);
emit_encode(lcmgen, f, lr);
emit_decode(lcmgen, f, lr);
emit_encoded_size(lcmgen, f, lr);
emit_get_hash(lcmgen, f, lr);
emit(0, "const char* %s::getTypeName()", lr->structname->shortname);
emit(0, "{");
emit(1, "return \"%s\";", lr->structname->shortname);
emit(0, "}");
emit(0, "");
emit_encode_nohash(lcmgen, f, lr);
emit_decode_nohash(lcmgen, f, lr);
emit_encoded_size_nohash(lcmgen, f, lr);
emit_compute_hash(lcmgen, f, lr);
emit_compare(lcmgen, f, lr);
emit(0, "/// Testing (ignores dynamic arrays & affects only first and last elements of static arrays)");
emit(0, "/// {");
emit_print(lcmgen, f, lr);
emit_init_test(lcmgen, f, lr);
emit_inc_test(lcmgen, f, lr);
emit_compare_test(lcmgen, f, lr);
emit(0, "/// }");
emit_package_namespace_close(lcmgen, f, lr);
emit(0, "#endif");
fclose(f);
}
g_free(header_name);
free(tn_);
}
return 0;
}
static char * get_file_name(lcmgen_t * lcmgen, lcm_struct_t const * lr, char const * func)
{
char const * tn = lr->structname->lctypename;
char * tn_ = dots_to_slashes(tn);
// compute the target filename
char * file_name = g_strdup_printf("%s%s%s%s.m",
getopt_get_string(lcmgen->gopt, "mpath"),
strlen(getopt_get_string(lcmgen->gopt, "mpath")) > 0 ? G_DIR_SEPARATOR_S : "",
tn_,
func);
free(tn_);
return file_name;
}
int
getopt_get_bool (getopt_t *getopt, const char *lname)
{
const char *v = getopt_get_string (getopt, lname);
assert (v!=NULL);
int val = 0==strcmp (v, "true");
return val;
}
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);
}
const char *make_fqn(zcmgen_t *zcm, const char *type_name)
{
if (strchr(type_name, '.')!=NULL)
return type_name;
if (!jdefaultpkg_warned && !getopt_was_specified(zcm->gopt, "jdefaultpkg")) {
printf("Notice: enclosing ZCM types without package into java namespace '%s'.\n", getopt_get_string(zcm->gopt, "jdefaultpkg"));
jdefaultpkg_warned = 1;
}
return g_strdup_printf("%s.%s", getopt_get_string(zcm->gopt, "jdefaultpkg"), type_name);
}
int
getopt_get_int (getopt_t *getopt, const char *lname)
{
const char *v = getopt_get_string (getopt, lname);
assert (v != NULL);
int errno = 0;
char *endptr = (char *) v;
int 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 val;
}
double
getopt_get_double (getopt_t *getopt, const char *lname)
{
const char *v = getopt_get_string (getopt, lname);
assert (v!=NULL);
int 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;
}
static int
emit_package (lcmgen_t *lcm, _package_contents_t *pc)
{
// create the package directory, if necessary
char **dirs = g_strsplit (pc->name, ".", 0);
char *pdname = build_filenamev (dirs);
char package_dir[PATH_MAX];
char package_dir_prefix[PATH_MAX];
int have_package = dirs[0] != NULL;
int write_init_py = !getopt_get_bool(lcm->gopt, "python-no-init");
sprintf (package_dir_prefix, "%s%s", getopt_get_string(lcm->gopt, "ppath"),
strlen(getopt_get_string(lcm->gopt, "ppath")) > 0 ?
G_DIR_SEPARATOR_S : "");
sprintf(package_dir, "%s%s%s", package_dir_prefix, pdname,
have_package ? G_DIR_SEPARATOR_S : "");
free (pdname);
if (strlen (package_dir)) {
if (! g_file_test (package_dir, G_FILE_TEST_EXISTS)) {
// g_mkdir_with_parents (package_dir, 0755);
mkdir_with_parents (package_dir, 0755);
}
if (!g_file_test (package_dir, G_FILE_TEST_IS_DIR)) {
err ("Could not create directory %s\n", package_dir);
return -1;
}
}
// write the package __init__.py files, if necessary
FILE *init_py_fp = NULL;
GHashTable * init_py_imports = g_hash_table_new_full(g_str_hash,
g_str_equal, free, NULL);
if (have_package && write_init_py) {
int ndirs = 0;
for (ndirs=0; dirs[ndirs]; ndirs++);
for (int i=0 ; i<ndirs; i++) {
char *initpy_fname_parts[1024];
assert(ndirs + 4 < 1024);
initpy_fname_parts[0] = package_dir_prefix;
for (int j=0; j<=i; j++) {
initpy_fname_parts[j+1] = dirs[j];
}
initpy_fname_parts[i+2] = "__init__.py";
initpy_fname_parts[i+3] = NULL;
char *initpy_fname = build_filenamev (initpy_fname_parts);
int created_initpy = 0;
// close init_py_fp if already open
if(init_py_fp) {
fclose(init_py_fp);
init_py_fp = NULL;
}
if (! g_file_test (initpy_fname, G_FILE_TEST_EXISTS)) {
// __init__.py does not exist for this package. Create it.
created_initpy = 1;
init_py_fp = fopen(initpy_fname, "w");
} else {
// open the existing __init__.py file, and make note of the
// modules it imports
created_initpy = 0;
init_py_fp = fopen(initpy_fname, "r+");
}
if (!init_py_fp) {
perror ("fopen");
free (initpy_fname);
return -1;
}
#ifndef WIN32
// lock __init__.py for exclusive write access
// TODO do the equivalent in windows
struct flock lockinfo;
lockinfo.l_type = F_WRLCK;
lockinfo.l_start = 0;
lockinfo.l_whence = SEEK_SET;
lockinfo.l_len = 0 ;
lockinfo.l_pid = getpid();
if(0 != fcntl(fileno(init_py_fp), F_SETLKW, &lockinfo)) {
perror("locking __init__.py");
free(initpy_fname);
fclose(init_py_fp);
return -1;
}
#endif
if(created_initpy) {
fprintf (init_py_fp, "\"\"\"LCM package __init__.py file\n"
"This file automatically generated by lcm-gen.\n"
"DO NOT MODIFY BY HAND!!!!\n"
"\"\"\"\n\n");
} else {
while(!feof(init_py_fp)) {
char buf[4096];
memset(buf, 0, sizeof(buf));
char *result = fgets(buf, sizeof(buf)-1, init_py_fp);
if(!result)
break;
g_strstrip(buf);
char **words = g_strsplit(buf, " ", -1);
if(!words[0] || !words[1] || !words[2] || !words[3])
continue;
if(!strcmp(words[0], "from") && !strcmp(words[2], "import")) {
char *module_name = strdup(words[1]+1); // ignore leading dot
g_hash_table_replace(init_py_imports, module_name,
module_name);
}
g_strfreev(words);
}
}
free (initpy_fname);
}
}
g_strfreev (dirs);
////////////////////////////////////////////////////////////
// ENUMS
for (int i=0; i<pc->enums->len; i++) {
lcm_enum_t *le = (lcm_enum_t *) g_ptr_array_index (pc->enums, i);
char path[PATH_MAX];
sprintf (path, "%s%s.py", package_dir, le->enumname->shortname);
if(init_py_fp &&
!g_hash_table_lookup(init_py_imports, le->enumname->shortname))
fprintf(init_py_fp, "from .%s import %s\n",
le->enumname->shortname,
le->enumname->shortname);
if (!lcm_needs_generation(lcm, le->lcmfile, path))
continue;
FILE *f = fopen(path, "w");
if (f==NULL) return -1;
fprintf(f, "\"\"\"LCM type definitions\n"
"This file automatically generated by lcm.\n"
"DO NOT MODIFY BY HAND!!!!\n"
"\"\"\"\n"
"\n"
"try:\n"
" import cStringIO.StringIO as BytesIO\n"
"except ImportError:\n"
" from io import BytesIO\n"
"import struct\n\n");
// enums always encoded as int32
emit (0, "class %s(object):", le->enumname->shortname);
emit (1, "__slots__ = [ \"value\" ]");
for (unsigned int v = 0; v < le->values->len; v++) {
lcm_enum_value_t *lev = (lcm_enum_value_t *) g_ptr_array_index(le->values, v);
emit(1, "%s = %i", lev->valuename, lev->value);
}
emit (1, "_packed_fingerprint = struct.pack(\">Q\", 0x%"PRIx64")",
le->hash);
fprintf (f, "\n");
emit (1, "def __init__ (self, value):");
emit (2, "self.value = value");
fprintf (f, "\n");
emit (1, "def _get_hash_recursive(parents):");
emit (2, "return 0x%"PRIx64, le->hash);
emit (1, "_get_hash_recursive=staticmethod(_get_hash_recursive)");
emit (1, "def _get_packed_fingerprint():");
emit (2, "return %s._packed_fingerprint", le->enumname->shortname);
emit (1, "_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)");
fprintf (f, "\n");
emit (1, "def encode(self):");
emit (2, "return struct.pack(\">Qi\", 0x%"PRIx64", self.value)",
le->hash);
emit (1, "def _encode_one(self, buf):");
emit (2, "buf.write (struct.pack(\">i\", self.value))");
fprintf (f, "\n");
emit (1, "def decode(data):");
emit (2, "if hasattr (data, 'read'):");
emit (3, "buf = data");
emit (2, "else:");
emit (3, "buf = BytesIO(data)");
emit (2, "if buf.read(8) != %s._packed_fingerprint:",
le->enumname->shortname);
emit (3, "raise ValueError(\"Decode error\")");
emit (2, "return %s(struct.unpack(\">i\", buf.read(4))[0])",
le->enumname->shortname);
emit (1, "decode = staticmethod(decode)");
emit (1, "def _decode_one(buf):");
emit (2, "return %s(struct.unpack(\">i\", buf.read(4))[0])",
le->enumname->shortname);
emit (1, "_decode_one = staticmethod(_decode_one)");
fprintf (f, "\n");
fclose (f);
}
////////////////////////////////////////////////////////////
// STRUCTS
for (int i = 0; i<pc->structs->len; i++) {
lcm_struct_t *ls = (lcm_struct_t *) g_ptr_array_index(pc->structs, i);
char path[PATH_MAX];
sprintf (path, "%s%s.py", package_dir, ls->structname->shortname);
if(init_py_fp &&
!g_hash_table_lookup(init_py_imports, ls->structname->shortname))
fprintf(init_py_fp, "from .%s import %s\n",
ls->structname->shortname,
ls->structname->shortname);
if (!lcm_needs_generation(lcm, ls->lcmfile, path))
continue;
FILE *f = fopen(path, "w");
if (f==NULL) return -1;
fprintf(f, "\"\"\"LCM type definitions\n"
"This file automatically generated by lcm.\n"
"DO NOT MODIFY BY HAND!!!!\n"
"\"\"\"\n"
"\n"
"try:\n"
" import cStringIO.StringIO as BytesIO\n"
"except ImportError:\n"
" from io import BytesIO\n"
"import struct\n\n");
emit_python_dependencies (lcm, f, ls);
fprintf(f, "class %s(object):\n", ls->structname->shortname);
fprintf (f," __slots__ = [");
for (unsigned int member = 0; member < ls->members->len; member++) {
lcm_member_t *lm = (lcm_member_t *) g_ptr_array_index (ls->members, member);
fprintf (f, "\"%s\"%s", lm->membername,
member < ls->members->len-1 ? ", " : "");
}
fprintf (f, "]\n\n");
// CONSTANTS
for (unsigned int cn = 0; cn < g_ptr_array_size(ls->constants); cn++) {
lcm_constant_t *lc = (lcm_constant_t *) g_ptr_array_index(ls->constants, cn);
assert(lcm_is_legal_const_type(lc->lctypename));
emit(1, "%s = %s", lc->membername, lc->val_str);
}
if (g_ptr_array_size(ls->constants) > 0)
emit(0, "");
emit_python_init (lcm, f, ls);
emit_python_encode (lcm, f, ls);
emit_python_encode_one (lcm, f, ls);
emit_python_decode (lcm, f, ls);
emit_python_decode_one (lcm, f, ls);
emit_python_fingerprint (lcm, f, ls);
fclose (f);
}
if(init_py_fp)
fclose(init_py_fp);
g_hash_table_destroy(init_py_imports);
return 0;
}
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;
}
// XXX step 2, basically the main function
static int
emit_package (lcmgen_t *lcm, _package_contents_t *pc)
{
// create the package directory, if necessary
char **dirs = g_strsplit (pc->name, ".", 0);
char *pdname = build_filenamev (dirs);
char package_dir[PATH_MAX];
char package_dir_prefix[PATH_MAX];
int have_package = dirs[0] != NULL;
sprintf (package_dir_prefix, "%s%s", getopt_get_string(lcm->gopt, "lpath"),
strlen(getopt_get_string(lcm->gopt, "lpath")) > 0 ?
G_DIR_SEPARATOR_S : "");
sprintf(package_dir, "%s%s%s", package_dir_prefix, pdname,
have_package ? G_DIR_SEPARATOR_S : "");
free (pdname);
if (strlen (package_dir)) {
if (! g_file_test (package_dir, G_FILE_TEST_EXISTS)) {
// g_mkdir_with_parents (package_dir, 0755);
mkdir_with_parents (package_dir, 0755);
}
if (!g_file_test (package_dir, G_FILE_TEST_IS_DIR)) {
err ("Could not create directory %s\n", package_dir);
return -1;
}
}
// write the package init.lua files, if necessary
FILE *init_lua_fp = NULL;
GHashTable * initlua_requires = NULL;
GHashTable * initlua_requires_subpack = NULL;
if (have_package) {
int ndirs = 0;
for (ndirs=0; dirs[ndirs]; ndirs++);
for (int i=0 ; i<ndirs; i++) {
// make filename
char *initlua_fname;
{
char *initlua_fname_parts[1024];
assert(ndirs + 4 < 1024);
initlua_fname_parts[0] = package_dir_prefix;
for (int j=0; j<=i; j++) {
initlua_fname_parts[j+1] = dirs[j];
}
initlua_fname_parts[i+2] = "init.lua";
initlua_fname_parts[i+3] = NULL;
initlua_fname = build_filenamev (initlua_fname_parts);
}
// make current package name
char * package_name;
{
char * name_parts[1024];
assert(i < 1024);
for (int j = 0; j <= i; j++) {
name_parts[j] = dirs[j];
}
name_parts[i + 1] = NULL;
package_name = g_strjoinv(".", name_parts);
}
if (initlua_requires) {
g_hash_table_destroy(initlua_requires);
initlua_requires = NULL;
}
if (initlua_requires_subpack) {
g_hash_table_destroy(initlua_requires_subpack);
initlua_requires_subpack = NULL;
}
initlua_requires = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, g_free);
initlua_requires_subpack = g_hash_table_new_full(g_str_hash, g_str_equal, g_free, g_free);
// if the file already exists, read the contents
if (g_file_test (initlua_fname, G_FILE_TEST_EXISTS)) {
init_lua_fp = fopen(initlua_fname, "r");
if (!init_lua_fp) {
perror ("fopen");
free (initlua_fname);
g_free(package_name);
return -1;
}
while(!feof(init_lua_fp)) {
char buf[4096];
memset(buf, 0, sizeof(buf));
char *result = fgets(buf, sizeof(buf)-1, init_lua_fp);
if(!result)
break;
// XXX get all of the previous types and packages
// this regex works because the first part is greedy
GRegex * regex = g_regex_new("require\\('([\\w+\\.]*\\.)(\\w+)'\\)( -- subpackage)?",
(GRegexCompileFlags) 0, (GRegexMatchFlags) 0, NULL);
GMatchInfo * matchinfo;
if(g_regex_match(regex, buf, (GRegexMatchFlags) 0, &matchinfo)){
if(g_match_info_get_match_count(matchinfo) == 3){
// not a subpackage
gchar * classname = g_match_info_fetch(matchinfo, 2);
g_hash_table_insert(initlua_requires, g_strdup(classname), g_strdup(classname));
}else if(g_match_info_get_match_count(matchinfo) == 4){
// this is a subpackage
// XXX fprintf(stderr, "> buff: %s\n", buf);
gchar * superpackage = g_match_info_fetch(matchinfo, 1);
gchar * subpackage = g_match_info_fetch(matchinfo, 2);
// XXX fprintf(stderr, "> super: %s, sub: %s\n", superpackage, subpackage);
gchar * fullsubpackage = g_strjoin("", superpackage, subpackage, NULL);
// XXX fprintf(stderr, "> [2] inserting: %s\n", fullsubpackage);
g_hash_table_insert(initlua_requires_subpack, g_strdup(fullsubpackage), g_strdup(fullsubpackage));
g_free(fullsubpackage);
}
}
g_match_info_free(matchinfo);
g_regex_unref(regex);
}
fclose(init_lua_fp);
init_lua_fp = NULL;
}
init_lua_fp = fopen(initlua_fname, "w");
// XXX fprintf(stderr, "> opened: %s\n", initlua_fname);
if (!init_lua_fp) {
perror ("fopen");
free (initlua_fname);
g_free(package_name);
return -1;
}
#ifndef WIN32
// lock init.lua for exclusive write access
// TODO do the equivalent in windows
struct flock lockinfo;
lockinfo.l_type = F_WRLCK;
lockinfo.l_start = 0;
lockinfo.l_whence = SEEK_SET;
lockinfo.l_len = 0 ;
lockinfo.l_pid = getpid();
if(0 != fcntl(fileno(init_lua_fp), F_SETLKW, &lockinfo)) {
perror("locking init.lua");
free(initlua_fname);
g_free(package_name);
fclose(init_lua_fp);
return -1;
}
#endif
fprintf (init_lua_fp, "--[[\n"
"LCM package init.lua file\n"
"This file automatically generated by lcm-gen.\n"
"DO NOT MODIFY BY HAND!!!!\n"
"--]]\n"
"\n"
"local M = {}\n"
"\n");
// add in all previous types
GList * package_types = g_hash_table_get_values(initlua_requires);
for (int j = 0; j < g_list_length(package_types); j++) {
char * tn = (char *) g_list_nth_data(package_types, j);
char * fn = g_strjoin(".", package_name, tn, NULL);
fprintf(init_lua_fp, "M.%s = require('%s')\n", tn, fn);
g_free(fn);
}
g_list_free(package_types);
// add in all previous packages
GList * subpacks = g_hash_table_get_values(initlua_requires_subpack);
for (int j = 0; j < g_list_length(subpacks); j++) {
char * spn = (char *) g_list_nth_data(subpacks, j);
// get the base of the package name
char ** tmpsplit = g_strsplit(spn, ".", -1);
char * sn = tmpsplit[g_strv_length(tmpsplit) - 1];
// XXX fprintf(stderr, "[1] sn: %s, spn: %s\n", sn, spn);
fprintf(init_lua_fp, "M.%s = require('%s') -- subpackage\n", sn, spn);
g_strfreev(tmpsplit);
}
g_list_free(subpacks);
// if the current package has a subpackage (which eventually contains the target package)
// add a `require` for that subpackage to the current (if it hasn't already)
if (i + 1 < ndirs) {
char *subpack_name = g_strjoin(".", package_name, dirs[i + 1], NULL);
// check for the subpackage name
if (!g_hash_table_lookup(initlua_requires_subpack, subpack_name)) {
// add it if it didn't exist
g_hash_table_insert(initlua_requires_subpack, g_strdup(subpack_name), g_strdup(subpack_name));
// XXX fprintf(stderr, "[2] sn: %s, spn: %s\n", dirs[i + 1], subpack_name);
fprintf(init_lua_fp, "M.%s = require('%s') -- subpackage\n", dirs[i + 1], subpack_name);
}
g_free(subpack_name);
}
// not yet the target?
if (i + 1 < ndirs) {
// close it out
fprintf(init_lua_fp, "\nreturn M\n\n");
fclose(init_lua_fp);
init_lua_fp = NULL;
}
free (initlua_fname);
g_free(package_name);
}
}
g_strfreev (dirs);
////////////////////////////////////////////////////////////
// STRUCTS
for (int i = 0; i<pc->structs->len; i++) {
lcm_struct_t *ls = (lcm_struct_t *) g_ptr_array_index(pc->structs, i);
char path[PATH_MAX];
sprintf (path, "%s%s.lua", package_dir, ls->structname->shortname);
if(init_lua_fp){
// XXX add the 'require' to the appropriate init.lua
if (!g_hash_table_lookup(initlua_requires, ls->structname->shortname)) {
fprintf(init_lua_fp, "M.%s = require('%s')\n", ls->structname->shortname, ls->structname->lctypename);
}
// XXX look for subpackages
for (unsigned int m = 0; m < g_ptr_array_size(ls->members); m++) {
lcm_member_t *lm = (lcm_member_t *) g_ptr_array_index(ls->members, m);
if(g_str_has_prefix(lm->type->package, pc->name)){
// make a regex starting with the current package...
gchar ** tmpsplit = g_strsplit(pc->name, ".", 0);
gchar * regexpackage = g_strjoinv("\\.", tmpsplit);
// only look for immediate submodules, not submodules of the submodules
gchar * regexstr = g_strjoin("", "^", regexpackage, "\\.(\\w+)", NULL);
GRegex * regex = g_regex_new(regexstr, (GRegexCompileFlags) 0, (GRegexMatchFlags) 0, NULL);
GMatchInfo * matchinfo;
g_strfreev(tmpsplit);
g_free(regexpackage);
g_free(regexstr);
if (g_regex_match(regex, lm->type->package, (GRegexMatchFlags) 0, &matchinfo)) {
if (g_match_info_get_match_count(matchinfo) == 2) {
gchar * fullsubpackage = g_match_info_fetch(matchinfo, 0);
gchar * subpackage = g_match_info_fetch(matchinfo, 1);
// was it already in the file?
if (!g_hash_table_lookup(initlua_requires_subpack, fullsubpackage)) {
// XXX fprintf(stderr, "> [1] inserting: %s\n", fullsubpackage);
g_hash_table_insert(initlua_requires_subpack, g_strdup(fullsubpackage), g_strdup(fullsubpackage));
fprintf(init_lua_fp, "M.%s = require('%s') -- subpackage\n", subpackage, fullsubpackage);
}
}
}
g_match_info_free(matchinfo);
g_regex_unref(regex);
}
}
}
if (!lcm_needs_generation(lcm, ls->lcmfile, path))
continue;
FILE *f = fopen(path, "w");
if (f==NULL) return -1;
fprintf(f, "--[[\n"
"LCM type definitions\n"
"This file automatically generated by lcm.\n"
"DO NOT MODIFY BY HAND!!!!\n"
"--]]\n"
"\n"
"local lcm = require('lcm')\n\n");
emit_lua_dependencies (lcm, f, ls);
// XXX added this...
emit_lua_locals(lcm, f, ls);
emit_lua_buffer_helper(lcm, f, ls);
// XXX step 3, start making the object
emit(0, "local %s = {}", ls->structname->shortname);
emit(0, "%s.__index = %s", ls->structname->shortname, ls->structname->shortname);
emit(0, "");
// CONSTANTS
for (unsigned int cn = 0; cn < g_ptr_array_size(ls->constants); cn++) {
lcm_constant_t *lc = (lcm_constant_t *) g_ptr_array_index(ls->constants, cn);
assert(lcm_is_legal_const_type(lc->lctypename));
emit(1, "%s.%s = %s", ls->structname->shortname,
lc->membername, lc->val_str);
}
if (g_ptr_array_size(ls->constants) > 0)
emit(0, "");
// NAMES
emit(0, "%s.name = '%s'", ls->structname->shortname, ls->structname->lctypename);
emit(0, "%s.packagename = '%s'", ls->structname->shortname, ls->structname->package);
emit(0, "%s.shortname = '%s'", ls->structname->shortname, ls->structname->shortname);
emit(0, "");
emit_lua_new (lcm, f, ls);
emit_lua_fingerprint (lcm, f, ls);
emit_lua_encode (lcm, f, ls);
emit_lua_encode_one (lcm, f, ls);
emit_lua_decode (lcm, f, ls);
emit_lua_decode_one (lcm, f, ls);
emit(0, "return %s", ls->structname->shortname);
emit(0, "");
fclose (f);
}
if(init_lua_fp){
fprintf(init_lua_fp, "\nreturn M\n\n");
fclose(init_lua_fp);
}
g_hash_table_destroy(initlua_requires);
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 emit_java(zcmgen_t *zcm)
{
GHashTable *type_table = g_hash_table_new(g_str_hash, g_str_equal);
g_hash_table_insert(type_table, "byte", prim("byte",
"# = ins.readByte();",
"outs.writeByte(#);"));
g_hash_table_insert(type_table, "int8_t", prim("byte",
"# = ins.readByte();",
"outs.writeByte(#);"));
g_hash_table_insert(type_table, "int16_t", prim("short",
"# = ins.readShort();",
"outs.writeShort(#);"));
g_hash_table_insert(type_table, "int32_t", prim("int",
"# = ins.readInt();",
"outs.writeInt(#);"));
g_hash_table_insert(type_table, "int64_t", prim("long",
"# = ins.readLong();",
"outs.writeLong(#);"));
g_hash_table_insert(type_table, "string", prim("String",
"__strbuf = new char[ins.readInt()-1]; for (int _i = 0; _i < __strbuf.length; _i++) __strbuf[_i] = (char) (ins.readByte()&0xff); ins.readByte(); # = new String(__strbuf);",
"__strbuf = new char[#.length()]; #.getChars(0, #.length(), __strbuf, 0); outs.writeInt(__strbuf.length+1); for (int _i = 0; _i < __strbuf.length; _i++) outs.write(__strbuf[_i]); outs.writeByte(0);"));
// g_hash_table_insert(type_table, "string", prim("String",
// "__strbuf = new byte[ins.readInt()-1]; ins.readFully(__strbuf); ins.readByte(); # = new String(__strbuf, \"UTF-8\");",
// "__strbuf = #.getBytes(\"UTF-8\"); outs.writeInt(__strbuf.length+1); outs.write(__strbuf, 0, __strbuf.length); outs.writeByte(0);"));
g_hash_table_insert(type_table, "boolean", prim("boolean",
"# = ins.readByte()!=0;",
"outs.writeByte( # ? 1 : 0);"));
g_hash_table_insert(type_table, "float", prim("float",
"# = ins.readFloat();",
"outs.writeFloat(#);"));
g_hash_table_insert(type_table, "double", prim("double",
"# = ins.readDouble();",
"outs.writeDouble(#);"));
//////////////////////////////////////////////////////////////
// ENUMS
for (unsigned int en = 0; en < g_ptr_array_size(zcm->enums); en++) {
zcm_enum_t *le = (zcm_enum_t *) g_ptr_array_index(zcm->enums, en);
const char *classname = make_fqn(zcm, le->enumname->lctypename);
char *path = g_strdup_printf("%s%s%s.java",
getopt_get_string(zcm->gopt, "jpath"),
strlen(getopt_get_string(zcm->gopt, "jpath")) > 0 ? G_DIR_SEPARATOR_S : "",
dots_to_slashes(classname));
if (!zcm_needs_generation(zcm, le->zcmfile, path))
continue;
if (getopt_get_bool(zcm->gopt, "jmkdir"))
make_dirs_for_file(path);
FILE *f = fopen(path, "w");
if (f==NULL)
return -1;
if (strlen(le->enumname->package) > 0)
emit(0, "package %s;", le->enumname->package);
else
emit(0, "package %s;", getopt_get_string(zcm->gopt, "jdefaultpkg"));
emit(0, " ");
emit(0, "import java.io.*;");
emit(0, "import java.util.*;");
emit(0, " ");
emit(0, "public final class %s %s", le->enumname->shortname, getopt_get_string(zcm->gopt, "jdecl"));
emit(0, "{");
emit(1, "public int value;");
emit(0, " ");
for (unsigned int v = 0; v < g_ptr_array_size(le->values); v++) {
zcm_enum_value_t *lev = (zcm_enum_value_t *) g_ptr_array_index(le->values, v);
emit(1, "public static final int %-16s = %i;",
lev->valuename, lev->value);
}
emit(0," ");
emit(1,"public %s(int value) { this.value = value; }",
le->enumname->shortname);
emit(0," ");
emit(1,"public int getValue() { return value; }");
emit(0," ");
emit(1,"public void _encodeRecursive(DataOutput outs) throws IOException");
emit(1,"{");
emit(2,"outs.writeInt(this.value);");
emit(1,"}");
emit(0," ");
emit(1,"public void encode(DataOutput outs) throws IOException");
emit(1,"{");
emit(2,"outs.writeLong(ZCM_FINGERPRINT);");
emit(2,"_encodeRecursive(outs);");
emit(1,"}");
emit(0," ");
emit(1,"public static %s _decodeRecursiveFactory(DataInput ins) throws IOException", make_fqn(zcm, le->enumname->lctypename));
emit(1,"{");
emit(2,"%s o = new %s(0);", make_fqn(zcm, le->enumname->lctypename), make_fqn(zcm, le->enumname->lctypename));
emit(2,"o._decodeRecursive(ins);");
emit(2,"return o;");
emit(1,"}");
emit(0," ");
emit(1,"public void _decodeRecursive(DataInput ins) throws IOException");
emit(1,"{");
emit(2,"this.value = ins.readInt();");
emit(1,"}");
emit(0," ");
emit(1,"public %s(DataInput ins) throws IOException", le->enumname->shortname);
emit(1,"{");
emit(2,"long hash = ins.readLong();");
emit(2,"if (hash != ZCM_FINGERPRINT)");
emit(3, "throw new IOException(\"ZCM Decode error: bad fingerprint\");");
emit(2,"_decodeRecursive(ins);");
emit(1,"}");
emit(0," ");
emit(1,"public %s copy()", classname);
emit(1,"{");
emit(2,"return new %s(this.value);", classname);
emit(1,"}");
emit(0," ");
emit(1,"public static final long _hashRecursive(ArrayList<Class<?>> clss)");
emit(1,"{");
emit(2,"return ZCM_FINGERPRINT;");
emit(1,"}");
emit(0," ");
emit(1, "public static final long ZCM_FINGERPRINT = 0x%016"PRIx64"L;", le->hash);
emit(0, "}");
fclose(f);
}
for (unsigned int st = 0; st < g_ptr_array_size(zcm->structs); st++) {
zcm_struct_t *lr = (zcm_struct_t *) g_ptr_array_index(zcm->structs, st);
const char *classname = make_fqn(zcm, lr->structname->lctypename);
char *path = g_strdup_printf("%s%s%s.java",
getopt_get_string(zcm->gopt, "jpath"),
strlen(getopt_get_string(zcm->gopt, "jpath")) > 0 ? G_DIR_SEPARATOR_S : "",
dots_to_slashes(classname));
if (!zcm_needs_generation(zcm, lr->zcmfile, path))
continue;
if (getopt_get_bool(zcm->gopt, "jmkdir"))
make_dirs_for_file(path);
FILE *f = fopen(path, "w");
if (f==NULL)
return -1;
emit(0, "/* ZCM type definition class file\n"
" * This file was automatically generated by zcm-gen\n"
" * DO NOT MODIFY BY HAND!!!!\n"
" */\n");
if (strlen(lr->structname->package) > 0)
emit(0, "package %s;", lr->structname->package);
else
emit(0, "package %s;", getopt_get_string(zcm->gopt, "jdefaultpkg"));
emit(0, " ");
emit(0, "import java.io.*;");
if (0) {
// Determine if we even need the java.nio.* package.
int usenio = 0;
for (unsigned int member = 0; member < g_ptr_array_size(lr->members); member++) {
zcm_member_t *lm = (zcm_member_t *) g_ptr_array_index(lr->members, member);
primitive_info_t *pinfo = (primitive_info_t*) g_hash_table_lookup(type_table, lm->type->lctypename);
if (pinfo!=NULL && !strcmp(pinfo->storage, "float")) {
usenio = 1;
break;
}
}
if (usenio)
emit(0, "import java.nio.*;");
}
emit(0, "import java.util.*;");
emit(0, "import zcm.zcm.*;");
emit(0, " ");
emit(0, "public final class %s %s", lr->structname->shortname, getopt_get_string(zcm->gopt, "jdecl"));
emit(0, "{");
for (unsigned int member = 0; member < g_ptr_array_size(lr->members); member++) {
zcm_member_t *lm = (zcm_member_t *) g_ptr_array_index(lr->members, member);
primitive_info_t *pinfo = (primitive_info_t*) g_hash_table_lookup(type_table, lm->type->lctypename);
emit_start(1, "public ");
if (pinfo==NULL) {
emit_continue("%s", make_fqn(zcm, lm->type->lctypename));
} else {
emit_continue("%s", pinfo->storage);
}
emit_continue(" %s", lm->membername);
for (unsigned int i = 0; i < g_ptr_array_size(lm->dimensions); i++)
emit_continue("[]");
emit_end(";");
}
emit(0," ");
// public constructor
emit(1,"public %s()", lr->structname->shortname);
emit(1,"{");
// pre-allocate any fixed-size arrays.
for (unsigned int member = 0; member < g_ptr_array_size(lr->members); member++) {
zcm_member_t *lm = (zcm_member_t *) g_ptr_array_index(lr->members, member);
primitive_info_t *pinfo = (primitive_info_t*) g_hash_table_lookup(type_table, lm->type->lctypename);
if (g_ptr_array_size(lm->dimensions)==0 || !zcm_is_constant_size_array(lm))
continue;
emit_start(2, "%s = new ", lm->membername);
if (pinfo != NULL)
emit_continue("%s", pinfo->storage);
else
emit_continue("%s", make_fqn(zcm, lm->type->lctypename));
for (unsigned int i = 0; i < g_ptr_array_size(lm->dimensions); i++) {
zcm_dimension_t *dim = (zcm_dimension_t*) g_ptr_array_index(lm->dimensions, i);
emit_continue("[%s]", dim->size);
}
emit_end(";");
}
emit(1,"}");
emit(0," ");
emit(1, "public static final long ZCM_FINGERPRINT;");
emit(1, "public static final long ZCM_FINGERPRINT_BASE = 0x%016"PRIx64"L;", lr->hash);
emit(0," ");
//////////////////////////////////////////////////////////////
// CONSTANTS
for (unsigned int cn = 0; cn < g_ptr_array_size(lr->constants); cn++) {
zcm_constant_t *lc = (zcm_constant_t *) g_ptr_array_index(lr->constants, cn);
assert(zcm_is_legal_const_type(lc->lctypename));
if (!strcmp(lc->lctypename, "int8_t")) {
emit(1, "public static final byte %s = (byte) %s;", lc->membername, lc->val_str);
} else if (!strcmp(lc->lctypename, "int16_t")) {
emit(1, "public static final short %s = (short) %s;", lc->membername, lc->val_str);
} else if (!strcmp(lc->lctypename, "int32_t")) {
emit(1, "public static final int %s = %s;", lc->membername, lc->val_str);
} else if (!strcmp(lc->lctypename, "int64_t")) {
emit(1, "public static final long %s = %sL;", lc->membername, lc->val_str);
} else if (!strcmp(lc->lctypename, "float")) {
emit(1, "public static final float %s = %sf;", lc->membername, lc->val_str);
} else if (!strcmp(lc->lctypename, "double")) {
emit(1, "public static final double %s = %s;", lc->membername, lc->val_str);
} else {
assert(0);
}
}
if (g_ptr_array_size(lr->constants) > 0)
emit(0, "");
///////////////// compute fingerprint //////////////////
emit(1, "static {");
emit(2, "ZCM_FINGERPRINT = _hashRecursive(new ArrayList<Class<?>>());");
emit(1, "}");
emit(0, " ");
emit(1, "public static long _hashRecursive(ArrayList<Class<?>> classes)");
emit(1, "{");
emit(2, "if (classes.contains(%s.class))", make_fqn(zcm, lr->structname->lctypename));
emit(3, "return 0L;");
emit(0, " ");
emit(2, "classes.add(%s.class);", make_fqn(zcm, lr->structname->lctypename));
emit(2, "long hash = ZCM_FINGERPRINT_BASE");
for (unsigned int member = 0; member < g_ptr_array_size(lr->members); member++) {
zcm_member_t *lm = (zcm_member_t *) g_ptr_array_index(lr->members, member);
primitive_info_t *pinfo = (primitive_info_t*) g_hash_table_lookup(type_table, lm->type->lctypename);
if (pinfo)
continue;
emit(3, " + %s._hashRecursive(classes)", make_fqn(zcm, lm->type->lctypename));
}
emit(3,";");
emit(2, "classes.remove(classes.size() - 1);");
emit(2, "return (hash<<1) + ((hash>>63)&1);");
emit(1, "}");
emit(0, " ");
///////////////// encode //////////////////
emit(1,"public void encode(DataOutput outs) throws IOException");
emit(1,"{");
emit(2,"outs.writeLong(ZCM_FINGERPRINT);");
emit(2,"_encodeRecursive(outs);");
emit(1,"}");
emit(0," ");
emit(1,"public void _encodeRecursive(DataOutput outs) throws IOException");
emit(1,"{");
if(struct_has_string_member(lr))
emit(2, "char[] __strbuf = null;");
char accessor[1024];
for (unsigned int member = 0; member < g_ptr_array_size(lr->members); member++) {
zcm_member_t *lm = (zcm_member_t *) g_ptr_array_index(lr->members, member);
primitive_info_t *pinfo = (primitive_info_t*) g_hash_table_lookup(type_table, lm->type->lctypename);
make_accessor(lm, "this", accessor);
encode_recursive(zcm, lm, f, pinfo, accessor, 0);
emit(0," ");
}
emit(1,"}");
emit(0," ");
///////////////// decode //////////////////
// decoding constructors
emit(1, "public %s(byte[] data) throws IOException", lr->structname->shortname);
emit(1, "{");
emit(2, "this(new ZCMDataInputStream(data));");
emit(1, "}");
emit(0, " ");
emit(1,"public %s(DataInput ins) throws IOException", lr->structname->shortname);
emit(1,"{");
emit(2,"if (ins.readLong() != ZCM_FINGERPRINT)");
emit(3, "throw new IOException(\"ZCM Decode error: bad fingerprint\");");
emit(0," ");
emit(2,"_decodeRecursive(ins);");
emit(1,"}");
emit(0," ");
emit(1,"public static %s _decodeRecursiveFactory(DataInput ins) throws IOException", make_fqn(zcm, lr->structname->lctypename));
emit(1,"{");
emit(2,"%s o = new %s();", make_fqn(zcm, lr->structname->lctypename), make_fqn(zcm, lr->structname->lctypename));
emit(2,"o._decodeRecursive(ins);");
emit(2,"return o;");
emit(1,"}");
emit(0," ");
emit(1,"public void _decodeRecursive(DataInput ins) throws IOException");
emit(1,"{");
if(struct_has_string_member(lr))
emit(2, "char[] __strbuf = null;");
for (unsigned int member = 0; member < g_ptr_array_size(lr->members); member++) {
zcm_member_t *lm = (zcm_member_t *) g_ptr_array_index(lr->members, member);
primitive_info_t *pinfo = (primitive_info_t*) g_hash_table_lookup(type_table, lm->type->lctypename);
make_accessor(lm, "this", accessor);
// allocate an array if necessary
if (g_ptr_array_size(lm->dimensions) > 0) {
emit_start(2, "this.%s = new ", lm->membername);
if (pinfo != NULL)
emit_continue("%s", pinfo->storage);
else
emit_continue("%s", make_fqn(zcm, lm->type->lctypename));
for (unsigned int i = 0; i < g_ptr_array_size(lm->dimensions); i++) {
zcm_dimension_t *dim = (zcm_dimension_t*) g_ptr_array_index(lm->dimensions, i);
emit_continue("[(int) %s]", dim->size);
}
emit_end(";");
}
decode_recursive(zcm, lm, f, pinfo, accessor, 0);
emit(0," ");
}
emit(1,"}");
emit(0," ");
///////////////// copy //////////////////
emit(1,"public %s copy()", classname);
emit(1,"{");
emit(2,"%s outobj = new %s();", classname, classname);
for (unsigned int member = 0; member < g_ptr_array_size(lr->members); member++) {
zcm_member_t *lm = (zcm_member_t *) g_ptr_array_index(lr->members, member);
primitive_info_t *pinfo = (primitive_info_t*) g_hash_table_lookup(type_table, lm->type->lctypename);
make_accessor(lm, "", accessor);
// allocate an array if necessary
if (g_ptr_array_size(lm->dimensions) > 0) {
emit_start(2, "outobj.%s = new ", lm->membername);
if (pinfo != NULL)
emit_continue("%s", pinfo->storage);
else
emit_continue("%s", make_fqn(zcm, lm->type->lctypename));
for (unsigned int i = 0; i < g_ptr_array_size(lm->dimensions); i++) {
zcm_dimension_t *dim = (zcm_dimension_t*) g_ptr_array_index(lm->dimensions, i);
emit_continue("[(int) %s]", dim->size);
}
emit_end(";");
}
copy_recursive(zcm, lm, f, pinfo, accessor, 0);
emit(0," ");
}
emit(2,"return outobj;");
emit(1,"}");
emit(0," ");
////////
emit(0, "}\n");
fclose(f);
}
/* XXX deallocate our storage. unfinished since memory leaks are non-critical for zcm-gen.
hashtable_iterator_t *hit = hashtable_iterator_create(type_table);
hashtable_entry_t *entry;
while ((entry = hashtable_iterator_next(hit)) != NULL) {
free((char*) entry->value);
}
hashtable_iterator_destroy(hit);
hashtable_destroy(type_table);
*/
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_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)
{
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[])
{
// 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);
}
}
/** Emit header file **/
static void emit_header_start(lcmgen_t *lcmgen, FILE *f, lcm_struct_t *ls)
{
char *tn = ls->structname->lctypename;
char *sn = ls->structname->shortname;
char *tn_ = dots_to_underscores(tn);
emit_auto_generated_warning(f);
fprintf(f, "#include <lcm/lcm_coretypes.h>\n");
fprintf(f, "\n");
fprintf(f, "#ifndef __%s_hpp__\n", tn_);
fprintf(f, "#define __%s_hpp__\n", tn_);
fprintf(f, "\n");
// do we need to #include <vector> and/or <string>?
int emit_include_vector = 0;
int emit_include_string = 0;
for (unsigned int mind = 0; mind < g_ptr_array_size(ls->members); mind++) {
lcm_member_t *lm = (lcm_member_t *)g_ptr_array_index(ls->members, mind);
if (g_ptr_array_size(lm->dimensions) != 0 &&
!lcm_is_constant_size_array(lm) && !emit_include_vector) {
emit(0, "#include <vector>");
emit_include_vector = 1;
}
if(!emit_include_string &&
!strcmp(lm->type->lctypename, "string")) {
emit(0, "#include <string>");
emit_include_string = 1;
}
}
// include header files for other LCM types
for (unsigned int mind = 0; mind < g_ptr_array_size(ls->members); mind++) {
lcm_member_t *lm = (lcm_member_t *) g_ptr_array_index(ls->members, mind);
if (!lcm_is_primitive_type(lm->type->lctypename) &&
strcmp(lm->type->lctypename, ls->structname->lctypename)) {
char *other_tn = dots_to_slashes (lm->type->lctypename);
emit(0, "#include \"%s%s%s.hpp\"",
getopt_get_string(lcmgen->gopt, "cpp-include"),
strlen(getopt_get_string(lcmgen->gopt, "cpp-include"))>0 ? G_DIR_SEPARATOR_S : "",
other_tn);
free(other_tn);
}
}
fprintf(f, "\n");
emit_package_namespace_start(lcmgen, f, ls);
// define the class
emit(0, "");
emit_comment(f, 0, ls->comment);
emit(0, "class %s", sn);
emit(0, "{");
// data members
if(g_ptr_array_size(ls->members)) {
emit(1, "public:");
for (unsigned int mind = 0; mind < g_ptr_array_size(ls->members); mind++) {
lcm_member_t *lm = (lcm_member_t *) g_ptr_array_index(ls->members, mind);
emit_comment(f, 2, lm->comment);
char* mapped_typename = map_type_name(lm->type->lctypename);
int ndim = g_ptr_array_size(lm->dimensions);
if (ndim == 0) {
emit(2, "%-10s %s;", mapped_typename, lm->membername);
} else {
if (lcm_is_constant_size_array(lm)) {
emit_start(2, "%-10s %s", mapped_typename, lm->membername);
for (unsigned int d = 0; d < ndim; d++) {
lcm_dimension_t *ld = (lcm_dimension_t *) g_ptr_array_index(lm->dimensions, d);
emit_continue("[%s]", ld->size);
}
emit_end(";");
} else {
emit_start(2, "");
for (unsigned int d = 0; d < ndim; d++)
emit_continue("std::vector< ");
emit_continue("%s", mapped_typename);
for (unsigned int d = 0; d < ndim; d++)
emit_continue(" >");
emit_end(" %s;", lm->membername);
}
}
free(mapped_typename);
if (mind < g_ptr_array_size(ls->members) - 1) {
emit(0, "");
}
}
emit(0, "");
}
// constants
if (g_ptr_array_size(ls->constants) > 0) {
emit(1, "public:");
for (unsigned int i = 0; i < g_ptr_array_size(ls->constants); i++) {
lcm_constant_t *lc = (lcm_constant_t *) g_ptr_array_index(ls->constants, i);
assert(lcm_is_legal_const_type(lc->lctypename));
emit_comment(f, 2, lc->comment);
// For int32_t only, we emit enums instead of static const
// values because the former can be passed by reference while
// the latter cannot.
if (!strcmp(lc->lctypename, "int32_t")) {
emit(2, "enum { %s = %s };", lc->membername, lc->val_str);
} else {
const char *suffix = "";
if (!strcmp(lc->lctypename, "int64_t"))
suffix = "LL";
char* mapped_typename = map_type_name(lc->lctypename);
emit(2, "static const %-8s %s = %s%s;", mapped_typename,
lc->membername, lc->val_str, suffix);
free(mapped_typename);
}
}
emit(0, "");
}
emit(1, "public:");
emit(2, "/**");
emit(2, " * Encode a message into binary form.");
emit(2, " *");
emit(2, " * @param buf The output buffer.");
emit(2, " * @param offset Encoding starts at thie byte offset into @p buf.");
emit(2, " * @param maxlen Maximum number of bytes to write. This should generally be");
emit(2, " * equal to getEncodedSize().");
emit(2, " * @return The number of bytes encoded, or <0 on error.");
emit(2, " */");
emit(2, "inline int encode(void *buf, int offset, int maxlen) const;");
emit(0, "");
emit(2, "/**");
emit(2, " * Check how many bytes are required to encode this message.");
emit(2, " */");
emit(2, "inline int getEncodedSize() const;");
emit(0, "");
emit(2, "/**");
emit(2, " * Decode a message from binary form into this instance.");
emit(2, " *");
emit(2, " * @param buf The buffer containing the encoded message.");
emit(2, " * @param offset The byte offset into @p buf where the encoded message starts.");
emit(2, " * @param maxlen The maximum number of bytes to reqad while decoding.");
emit(2, " * @return The number of bytes decoded, or <0 if an error occured.");
emit(2, " */");
emit(2, "inline int decode(const void *buf, int offset, int maxlen);");
emit(0, "");
emit(2, "/**");
emit(2, " * Retrieve the 64-bit fingerprint identifying the structure of the message.");
emit(2, " * Note that the fingerprint is the same for all instances of the same");
emit(2, " * message type, and is a fingerprint on the message type definition, not on");
emit(2, " * the message contents.");
emit(2, " */");
emit(2, "inline static int64_t getHash();");
emit(0, "");
emit(2, "/**");
emit(2, " * Returns \"%s\"", ls->structname->shortname);
emit(2, " */");
emit(2, "inline static const char* getTypeName();");
emit(0, "");
emit(2, "// LCM support functions. Users should not call these");
emit(2, "inline int _encodeNoHash(void *buf, int offset, int maxlen) const;");
emit(2, "inline int _getEncodedSizeNoHash() const;");
emit(2, "inline int _decodeNoHash(const void *buf, int offset, int maxlen);");
emit(2, "inline static int64_t _computeHash(const __lcm_hash_ptr *p);");
emit(0, "};");
emit(0, "");
free(tn_);
}
int emit_struct(lcmgen_t *lcmgen, lcm_struct_t *lr)
{
char *tn = lr->structname->lctypename;
char *tn_ = dots_to_underscores(tn);
char *header_name = g_strdup_printf("%s/%s.h", getopt_get_string(lcmgen->gopt, "c-hpath"), tn_);
char *c_name = g_strdup_printf("%s/%s.c", getopt_get_string(lcmgen->gopt, "c-cpath"), tn_);
if (lcm_needs_generation(lcmgen, lr->lcmfile, header_name)) {
FILE *f = fopen(header_name, "w");
if (f == NULL)
return -1;
emit_header_top(lcmgen, f, tn_);
emit_header_struct(lcmgen, f, lr);
emit_header_prototypes(lcmgen, f, lr);
emit_header_bottom(lcmgen, f);
fclose(f);
}
// STRUCT C file
if (lcm_needs_generation(lcmgen, lr->lcmfile, c_name)) {
FILE *f = fopen(c_name, "w");
if (f == NULL)
return -1;
emit_auto_generated_warning(f);
fprintf(f, "#include <string.h>\n");
fprintf(f, "#include \"%s%s%s.h\"\n",
getopt_get_string(lcmgen->gopt, "cinclude"),
strlen(getopt_get_string(lcmgen->gopt, "cinclude"))>0 ? "/" : "",
tn_);
fprintf(f, "\n");
emit_c_struct_get_hash(lcmgen, f, lr);
emit_c_encode_array(lcmgen, f, lr);
emit_c_encode(lcmgen, f, lr);
emit_c_encoded_array_size(lcmgen, f, lr);
emit_c_encoded_size(lcmgen, f, lr);
if(getopt_get_bool(lcmgen->gopt, "c-typeinfo")) {
emit_c_struct_size(lcmgen, f, lr);
emit_c_num_fields(lcmgen, f, lr);
emit_c_get_field(lcmgen, f, lr);
emit_c_get_type_info(lcmgen, f, lr);
}
emit_c_decode_array(lcmgen, f, lr);
emit_c_decode_array_cleanup(lcmgen, f, lr);
emit_c_decode(lcmgen, f, lr);
emit_c_decode_cleanup(lcmgen, f, lr);
emit_c_clone_array(lcmgen, f, lr);
emit_c_copy(lcmgen, f, lr);
emit_c_destroy(lcmgen, f, lr);
if(!getopt_get_bool(lcmgen->gopt, "c-no-pubsub")) {
emit_c_struct_publish(lcmgen, f, lr );
emit_c_struct_subscribe(lcmgen, f, lr );
}
fclose(f);
}
return 0;
}
int emit_enum(lcmgen_t *lcmgen, lcm_enum_t *le)
{
char *tn = le->enumname->lctypename;
char *tn_ = dots_to_underscores(tn);
char *header_name = g_strdup_printf("%s/%s.h", getopt_get_string(lcmgen->gopt, "c-hpath"), tn_);
char *c_name = g_strdup_printf("%s/%s.c", getopt_get_string(lcmgen->gopt, "c-cpath"), tn_);
// ENUM header file
if (lcm_needs_generation(lcmgen, le->lcmfile, header_name)) {
FILE *f = fopen(header_name, "w");
if (f == NULL)
return -1;
emit_header_top(lcmgen, f, tn_);
char *tn_upper = g_ascii_strup (tn_, strlen (tn_));
///////////////////////////////////////////////////////////////////
// the enum declaration itself
emit(0, "enum _%s {", tn_);
for (unsigned int i = 0; i < g_ptr_array_size(le->values); i++) {
lcm_enum_value_t *lev = (lcm_enum_value_t *) g_ptr_array_index(le->values, i);
emit(1," %s_%s = %d%s",
tn_upper,
lev->valuename,
lev->value, i==(g_ptr_array_size(le->values)-1) ? "" : ",");
}
free (tn_upper);
emit(0, "};");
emit(0, "");
emit(0, "typedef enum _%s %s;", tn_, tn_);
emit(0, "");
emit(0, "const char * %s_name(%s val);", tn_, tn_);
emit(0, "");
///////////////////////////////////////////////////////////////////
emit(0, "static inline int64_t __%s_hash_recursive(const __lcm_hash_ptr *p)", tn_);
emit(0, "{");
emit(1, "return 0x%016"PRIx64"LL;", le->hash);
emit(0, "}");
emit(0, "");
emit(0, "static inline int64_t __%s_get_hash()", tn_);
emit(0, "{");
emit(1, "return 0x%016"PRIx64"LL;", le->hash);
emit(0, "}");
emit(0, "");
// enums are always "ints", but "ints" are not always int32_t. We
// always store an enum as an int32_t, however. Consequently, we
// jump through some hoops here in order to allow the compiler to
// convert from an int32_t to whatever the native size of "int"
// is.
emit(0, "static inline int __%s_encode_array(void *_buf, int offset, int maxlen, const %s *p, int elements)", tn_, tn_);
emit(0, "{");
emit(1, "int pos = 0, thislen, element;");
emit(1, "for (element = 0; element < elements; element++) {");
emit(2, "int32_t v = (int32_t) p[element];");
emit(2, "thislen = __int32_t_encode_array(_buf, offset + pos, maxlen - pos, &v, 1);");
emit(2, "if (thislen < 0) return thislen; else pos += thislen;");
emit(1, "}");
emit(1, "return thislen;");
emit(0, "}");
emit(0, "");
emit(0,"static inline int %s_encode(void *buf, int offset, int maxlen, const %s *p)", tn_, tn_);
emit(0,"{");
emit(1, "int pos = 0, thislen;");
emit(1, "int64_t hash = 0x%016"PRIx64"LL;", le->hash);
emit(0,"");
emit(1, "thislen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1);");
emit(1, "if (thislen < 0) return thislen; else pos += thislen;");
emit(0,"");
emit(1, "thislen = __%s_encode_array(buf, offset + pos, maxlen - pos, p, 1);", tn_);
emit(1, "if (thislen < 0) return thislen; else pos += thislen;");
emit(0,"");
emit(1, "return pos;");
emit(0,"}");
emit(0,"");
emit(0, "static inline int __%s_decode_array(const void *_buf, int offset, int maxlen, %s *p, int elements)", tn_, tn_);
emit(0, "{");
emit(1, "int pos = 0, thislen, element;");
emit(1, "for (element = 0; element < elements; element++) {");
emit(2, "int32_t v;");
emit(2, "thislen = __int32_t_decode_array(_buf, offset + pos, maxlen - pos, &v, 1);");
emit(2, "if (thislen < 0) return thislen; else pos += thislen;");
emit(2, "p[element] = (%s) v;", tn_);
emit(1, "}");
emit(1, "return thislen;");
emit(0, "}");
emit(0, "");
emit(0, "static inline int __%s_clone_array(const %s *p, %s *q, int elements)", tn_, tn_, tn_);
emit(0, "{");
emit(1, "memcpy(q, p, elements * sizeof(%s));", tn_);
emit(1, "return 0;");
emit(0, "}");
emit(0, "");
emit(0,"static inline int %s_decode(const void *buf, int offset, int maxlen, %s *p)", tn_, tn_);
emit(0,"{");
emit(1, "int pos = 0, thislen;");
emit(1, "int64_t hash = 0x%016"PRIx64"LL;", le->hash);
emit(0,"");
emit(1, "int64_t this_hash;");
emit(1, "thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &this_hash, 1);");
emit(1, "if (thislen < 0) return thislen; else pos += thislen;");
emit(1, "if (this_hash != hash) return -1;");
emit(0,"");
emit(1, "thislen = __%s_decode_array(buf, offset + pos, maxlen - pos, p, 1);", tn_);
emit(1, "if (thislen < 0) return thislen; else pos += thislen;");
emit(0,"");
emit(1, "return pos;");
emit(0,"}");
emit(0,"");
emit(0, "static inline int __%s_decode_array_cleanup(%s *in, int elements)", tn_, tn_);
emit(0, "{");
emit(1, "return 0;");
emit(0, "}");
emit(0, "");
emit(0,"static inline int %s_decode_cleanup(%s *p)", tn_, tn_);
emit(0,"{");
emit(1, "return 0;");
emit(0,"}");
emit(0,"");
emit(0, "static inline int __%s_encoded_array_size(const %s *p, int elements)", tn_, tn_);
emit(0, "{");
emit(1, "return __int32_t_encoded_array_size((const int32_t*)p, elements);");
emit(0, "}");
emit(0, "");
emit(0, "static inline int %s_encoded_size(const %s *in)", tn_, tn_);
emit(0, "{");
emit(1, "return int32_t_encoded_size((const int32_t*)in);");
emit(0, "}");
emit(0, "");
emit_header_bottom(lcmgen, f);
fclose(f);
}
// ENUM C file
if (lcm_needs_generation(lcmgen, le->lcmfile, c_name)) {
char *tn_upper = g_ascii_strup (tn_, strlen (tn_));
FILE *f = fopen(c_name, "w");
emit_auto_generated_warning(f);
fprintf(f, "#include \"%s%s%s.h\"\n",
getopt_get_string(lcmgen->gopt, "cinclude"),
strlen(getopt_get_string(lcmgen->gopt, "cinclude"))>0 ? "/" : "",
tn_);
emit(0, "const char * %s_name(%s val)", tn_, tn_);
emit(0, "{");
emit(1, "switch (val) {");
for (unsigned int i = 0; i < g_ptr_array_size(le->values); i++) {
lcm_enum_value_t *lev = (lcm_enum_value_t *) g_ptr_array_index(le->values, i);
emit(2, "case %s_%s:", tn_upper, lev->valuename);
emit(3, "return \"%s\";", lev->valuename);
}
emit(2, "default:");
emit(3, "return NULL;");
emit(1, "}");
emit(0, "}");
fclose(f);
free (tn_upper);
}
return 0;
}
/** Emit header file output specific to a particular type of struct. **/
static void emit_header_struct(lcmgen_t *lcm, FILE *f, lcm_struct_t *ls)
{
char *tn = ls->structname->lctypename;
char *tn_ = dots_to_underscores(tn);
char *tn_upper = g_utf8_strup(tn_, -1);
// include header files required by members
for (unsigned int i = 0; i < g_ptr_array_size(ls->members); i++) {
lcm_member_t *lm = (lcm_member_t *) g_ptr_array_index(ls->members, i);
if (!lcm_is_primitive_type(lm->type->lctypename) &&
strcmp(lm->type->lctypename, ls->structname->lctypename)) {
char *other_tn = dots_to_underscores (lm->type->lctypename);
fprintf(f, "#include \"%s%s%s.h\"\n",
getopt_get_string(lcm->gopt, "cinclude"),
strlen(getopt_get_string(lcm->gopt, "cinclude"))>0 ? "/" : "",
other_tn);
free (other_tn);
}
}
// output constants
for (unsigned int i = 0; i < g_ptr_array_size(ls->constants); i++) {
lcm_constant_t *lc = (lcm_constant_t *) g_ptr_array_index(ls->constants, i);
assert(lcm_is_legal_const_type(lc->lctypename));
const char *suffix = "";
if (!strcmp(lc->lctypename, "int64_t")) {
suffix = "LL";
}
emit_comment(f, 0, lc->comment);
emit(0, "#define %s_%s %s%s", tn_upper, lc->membername, lc->val_str,
suffix);
}
if (g_ptr_array_size(ls->constants) > 0) {
emit(0, "");
}
// define the struct
emit_comment(f, 0, ls->comment);
emit(0, "typedef struct _%s %s;", tn_, tn_);
emit(0, "struct _%s", tn_);
emit(0, "{");
for (unsigned int m = 0; m < g_ptr_array_size(ls->members); m++) {
lcm_member_t *lm = (lcm_member_t *) g_ptr_array_index(ls->members, m);
emit_comment(f, 1, lm->comment);
int ndim = g_ptr_array_size(lm->dimensions);
if (ndim == 0) {
emit(1, "%-10s %s;", map_type_name(lm->type->lctypename),
lm->membername);
} else {
if (lcm_is_constant_size_array(lm)) {
emit_start(1, "%-10s %s", map_type_name(lm->type->lctypename), lm->membername);
for (unsigned int d = 0; d < ndim; d++) {
lcm_dimension_t *ld = (lcm_dimension_t *) g_ptr_array_index(lm->dimensions, d);
emit_continue("[%s]", ld->size);
}
emit_end(";");
} else {
emit_start(1, "%-10s ", map_type_name(lm->type->lctypename));
for (unsigned int d = 0; d < ndim; d++)
emit_continue("*");
emit_end("%s;", lm->membername);
}
}
}
emit(0, "};");
emit(0, "");
free(tn_);
g_free(tn_upper);
}
static void emit_header_prototypes(lcmgen_t *lcmgen, FILE *f, lcm_struct_t *ls)
{
char *xd = getopt_get_string(lcmgen->gopt, "c-export-symbol");
char *xd_ = add_space_or_empty(xd);
char *tn = ls->structname->lctypename;
char *tn_ = dots_to_underscores(tn);
emit(0, "/**");
emit(0, " * Create a deep copy of a %s.", tn_);
emit(0, " * When no longer needed, destroy it with %s_destroy()", tn_);
emit(0, " */");
emit(0,"%s%s* %s_copy(const %s* to_copy);", xd_, tn_, tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Destroy an instance of %s created by %s_copy()", tn_, tn_);
emit(0, " */");
emit(0,"%svoid %s_destroy(%s* to_destroy);", xd_, tn_, tn_);
emit(0,"");
if (!getopt_get_bool(lcmgen->gopt, "c-no-pubsub")) {
emit(0, "/**");
emit(0, " * Identifies a single subscription. This is an opaque data type.");
emit(0, " */");
emit(0,"typedef struct _%s_subscription_t %s_subscription_t;", tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Prototype for a callback function invoked when a message of type");
emit(0, " * %s is received.", tn_);
emit(0, " */");
emit(0,"typedef void(*%s_handler_t)(const lcm_recv_buf_t *rbuf,\n"
" const char *channel, const %s *msg, void *userdata);",
tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Publish a message of type %s using LCM.", tn_);
emit(0, " *");
emit(0, " * @param lcm The LCM instance to publish with.");
emit(0, " * @param channel The channel to publish on.");
emit(0, " * @param msg The message to publish.");
emit(0, " * @return 0 on success, <0 on error. Success means LCM has transferred");
emit(0, " * responsibility of the message data to the OS.");
emit(0, " */");
emit(0,"%sint %s_publish(lcm_t *lcm, const char *channel, const %s *msg);", xd_, tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Subscribe to messages of type %s using LCM.", tn_);
emit(0, " *");
emit(0, " * @param lcm The LCM instance to subscribe with.");
emit(0, " * @param channel The channel to subscribe to.");
emit(0, " * @param handler The callback function invoked by LCM when a message is received.");
emit(0, " * This function is invoked by LCM during calls to lcm_handle() and");
emit(0, " * lcm_handle_timeout().");
emit(0, " * @param userdata An opaque pointer passed to @p handler when it is invoked.");
emit(0, " * @return 0 on success, <0 if an error occured");
emit(0, " */");
emit(0,"%s%s_subscription_t* %s_subscribe(lcm_t *lcm, const char *channel, %s_handler_t handler, void *userdata);",
xd_, tn_, tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Removes and destroys a subscription created by %s_subscribe()", tn_);
emit(0, " */");
emit(0,"%sint %s_unsubscribe(lcm_t *lcm, %s_subscription_t* hid);", xd_, tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Sets the queue capacity for a subscription.");
emit(0, " * Some LCM providers (e.g., the default multicast provider) are implemented");
emit(0, " * using a background receive thread that constantly revceives messages from");
emit(0, " * the network. As these messages are received, they are buffered on");
emit(0, " * per-subscription queues until dispatched by lcm_handle(). This function");
emit(0, " * how many messages are queued before dropping messages.");
emit(0, " *");
emit(0, " * @param subs the subscription to modify.");
emit(0, " * @param num_messages The maximum number of messages to queue");
emit(0, " * on the subscription.");
emit(0, " * @return 0 on success, <0 if an error occured");
emit(0, " */");
emit(0,"%sint %s_subscription_set_queue_capacity(%s_subscription_t* subs,\n"
" int num_messages);\n", xd_, tn_, tn_);
}
emit(0, "/**");
emit(0, " * Encode a message of type %s into binary form.", tn_);
emit(0, " *");
emit(0, " * @param buf The output buffer.");
emit(0, " * @param offset Encoding starts at this byte offset into @p buf.");
emit(0, " * @param maxlen Maximum number of bytes to write. This should generally");
emit(0, " * be equal to %s_encoded_size().", tn_);
emit(0, " * @param msg The message to encode.");
emit(0, " * @return The number of bytes encoded, or <0 if an error occured.");
emit(0, " */");
emit(0,"%sint %s_encode(void *buf, int offset, int maxlen, const %s *p);", xd_, tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Decode a message of type %s from binary form.", tn_);
emit(0, " * When decoding messages containing strings or variable-length arrays, this");
emit(0, " * function may allocate memory. When finished with the decoded message,");
emit(0, " * release allocated resources with %s_decode_cleanup().", tn_);
emit(0, " *");
emit(0, " * @param buf The buffer containing the encoded message");
emit(0, " * @param offset The byte offset into @p buf where the encoded message starts.");
emit(0, " * @param maxlen The maximum number of bytes to read while decoding.");
emit(0, " * @param msg Output parameter where the decoded message is stored");
emit(0, " * @return The number of bytes decoded, or <0 if an error occured.");
emit(0, " */");
emit(0,"%sint %s_decode(const void *buf, int offset, int maxlen, %s *msg);", xd_, tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Release resources allocated by %s_decode()", tn_);
emit(0, " * @return 0");
emit(0, " */");
emit(0,"%sint %s_decode_cleanup(%s *p);", xd_, tn_, tn_);
emit(0, "");
emit(0, "/**");
emit(0, " * Check how many bytes are required to encode a message of type %s", tn_);
emit(0, " */");
emit(0,"%sint %s_encoded_size(const %s *p);", xd_, tn_, tn_);
if(getopt_get_bool(lcmgen->gopt, "c-typeinfo")) {
emit(0,"%ssize_t %s_struct_size(void);", xd_, tn_);
emit(0,"%sint %s_num_fields(void);", xd_, tn_);
emit(0,"%sint %s_get_field(const %s *p, int i, lcm_field_t *f);", xd_, tn_, tn_);
emit(0,"%sconst lcm_type_info_t *%s_get_type_info(void);", xd_, tn_);
}
emit(0,"");
emit(0,"// LCM support functions. Users should not call these");
emit(0,"%sint64_t __%s_get_hash(void);", xd_, tn_);
emit(0,"%suint64_t __%s_hash_recursive(const __lcm_hash_ptr *p);", xd_, tn_);
emit(0,"%sint __%s_encode_array(void *buf, int offset, int maxlen, const %s *p, int elements);", xd_, tn_, tn_);
emit(0,"%sint __%s_decode_array(const void *buf, int offset, int maxlen, %s *p, int elements);", xd_, tn_, tn_);
emit(0,"%sint __%s_decode_array_cleanup(%s *p, int elements);", xd_, tn_, tn_);
emit(0,"%sint __%s_encoded_array_size(const %s *p, int elements);", xd_, tn_, tn_);
emit(0,"%sint __%s_clone_array(const %s *p, %s *q, int elements);", xd_, tn_, tn_, tn_);
emit(0,"");
}
/** Emit header file **/
static void emit_header_start(lcmgen_t *lcmgen, FILE *f, lcm_struct_t *ls)
{
char *tn = ls->structname->lctypename;
char *sn = ls->structname->shortname;
char *tn_ = dots_to_underscores(tn);
emit_auto_generated_warning(f);
fprintf(f, "#include <lcm/lcm_coretypes.h>\n");
fprintf(f, "\n");
fprintf(f, "#ifndef __%s_hpp__\n", tn_);
fprintf(f, "#define __%s_hpp__\n", tn_);
fprintf(f, "\n");
// do we need to #include <vector> ?
int emit_include_vector = 0;
for (unsigned int mind = 0; mind < g_ptr_array_size(ls->members); mind++) {
lcm_member_t *lm = (lcm_member_t *)g_ptr_array_index(ls->members, mind);
if (g_ptr_array_size(lm->dimensions) != 0 &&
!lcm_is_constant_size_array(lm) && !emit_include_vector) {
emit(0, "#include <vector>");
emit_include_vector = 1;
}
}
emit(0, "#include <cmath>\t// For fabs to compare floats");
emit(0, "#include <string>\t// For return type of toStr()");
emit(0, "#include <sstream>\t// For stringstream in toStr()");
// include header files for other LCM types
for (unsigned int mind = 0; mind < g_ptr_array_size(ls->members); mind++) {
lcm_member_t *lm = (lcm_member_t *) g_ptr_array_index(ls->members, mind);
if (!lcm_is_primitive_type(lm->type->lctypename) &&
strcmp(lm->type->lctypename, ls->structname->lctypename)) {
char *other_tn = dots_to_slashes (lm->type->lctypename);
emit(0, "#include \"%s%s%s.hpp\"",
getopt_get_string(lcmgen->gopt, "cpp-include"),
strlen(getopt_get_string(lcmgen->gopt, "cpp-include"))>0 ? G_DIR_SEPARATOR_S : "",
other_tn);
free(other_tn);
}
}
fprintf(f, "\n");
emit_package_namespace_start(lcmgen, f, ls);
// define the class
emit(0, "");
emit_comment(f, 0, ls->comment);
emit(0, "class %s", sn);
emit(0, "{");
// data members
if(g_ptr_array_size(ls->members)) {
emit(1, "public:");
for (unsigned int mind = 0; mind < g_ptr_array_size(ls->members); mind++) {
lcm_member_t *lm = (lcm_member_t *) g_ptr_array_index(ls->members, mind);
emit_comment(f, 2, lm->comment);
char const* mapped_typename = map_type_name(lm->type->lctypename);
int ndim = g_ptr_array_size(lm->dimensions);
if (ndim == 0) {
emit(2, "%-10s %s;", mapped_typename, lm->membername);
} else {
if (lcm_is_constant_size_array(lm)) {
emit_start(2, "%-10s %s", mapped_typename, lm->membername);
for (unsigned int d = 0; d < ndim; d++) {
lcm_dimension_t *ld = (lcm_dimension_t *) g_ptr_array_index(lm->dimensions, d);
emit_continue("[%s]", ld->size);
}
emit_end(";");
} else {
emit_start(2, "");
for (unsigned int d = 0; d < ndim; d++)
emit_continue("std::vector< ");
emit_continue("%s", mapped_typename);
for (unsigned int d = 0; d < ndim; d++)
emit_continue(" >");
emit_end(" %s;", lm->membername);
}
}
if (mind < g_ptr_array_size(ls->members) - 1) {
emit(0, "");
}
}
emit(0, "");
}
// constants
if (g_ptr_array_size(ls->constants) > 0) {
emit(1, "public:");
for (unsigned int i = 0; i < g_ptr_array_size(ls->constants); i++) {
lcm_constant_t *lc = (lcm_constant_t *) g_ptr_array_index(ls->constants, i);
assert(lcm_is_legal_const_type(lc->lctypename));
emit_comment(f, 2, lc->comment);
// For int32_t only, we emit enums instead of static const
// values because the former can be passed by reference while
// the latter cannot.
if (!strcmp(lc->lctypename, "int32_t")) {
emit(2, "enum { %s = %s };", lc->membername, lc->val_str);
} else {
const char *suffix = "";
if (!strcmp(lc->lctypename, "int64_t"))
suffix = "LL";
char const* mapped_typename = map_type_name(lc->lctypename);
char *cpp_std = getopt_get_string(lcmgen->gopt, "cpp-std");
if(strcmp("c++98",cpp_std) && strcmp("c++11",cpp_std)) {
printf("%s is not a valid cpp_std. Use --cpp-std=c++98 or --cpp-std=c++11 instead\n\n", cpp_std);
fflush(stdout);
_exit(1);
}
if(!strcmp (cpp_std, "c++11")) {
emit(2, "static constexpr %-8s %s = %s%s;", mapped_typename,
lc->membername, lc->val_str, suffix);
} else {
emit(2, "// If you're using C++11 and are getting compiler errors saying things like");
emit(2, "// ‘constexpr’ needed for in-class initialization of static data member");
emit(2, "// then re-run lcm-gen with '--cpp-std=c++11' to generate code that is");
emit(2, "// compliant with C++11");
emit(2, "static const %-8s %s = %s%s;", mapped_typename,
lc->membername, lc->val_str, suffix);
}
}
}
emit(0, "");
}
emit(1, "public:");
emit(2, "/**");
emit(2, " * Encode a message into binary form.");
emit(2, " *");
emit(2, " * @param buf The output buffer.");
emit(2, " * @param offset Encoding starts at thie byte offset into @p buf.");
emit(2, " * @param maxlen Maximum number of bytes to write. This should generally be");
emit(2, " * equal to getEncodedSize().");
emit(2, " * @return The number of bytes encoded, or <0 on error.");
emit(2, " */");
emit(2, "inline int encode(void *buf, int offset, int maxlen) const;");
emit(0, "");
emit(2, "/**");
emit(2, " * Check how many bytes are required to encode this message.");
emit(2, " */");
emit(2, "inline int getEncodedSize() const;");
emit(0, "");
emit(2, "/**");
emit(2, " * Decode a message from binary form into this instance.");
emit(2, " *");
emit(2, " * @param buf The buffer containing the encoded message.");
emit(2, " * @param offset The byte offset into @p buf where the encoded message starts.");
emit(2, " * @param maxlen The maximum number of bytes to reqad while decoding.");
emit(2, " * @return The number of bytes decoded, or <0 if an error occured.");
emit(2, " */");
emit(2, "inline int decode(const void *buf, int offset, int maxlen);");
emit(0, "");
emit(2, "/**");
emit(2, " * Compare two instances of the same type.");
emit(2, " *");
emit(2, " * @param[in] other The object to compare.");
emit(2, " * @return true if all elements are equal, false otherwise.");
emit(2, " */");
emit(2, "inline bool operator==(%s const & other) const;", sn);
emit(0, "");
emit(2, "/**");
emit(2, " * Construct a string from the object for debugging purposes.");
emit(2, " * Note that arrays are cropped for readability. Only the first and last elements are shown.");
emit(2, " *");
emit(2, " * @param[in] layout A placeholder for the tree structure.");
emit(2, " */");
emit(2, "inline std::string toDebugStr(std::string const & layout=\"|-\") const;");
emit(0, "");
emit(2, "/**");
emit(2, " * Initialize all values of a test object to pre-defined values.");
emit(2, " * The values used are much more informative than using 0 because");
emit(2, " * they are type-dependant. This way, the user knows directly whether");
emit(2, " * the type is correct or not. Typically the default values are the");
emit(2, " * maximum value allowed by the type minus 5, and 1.0 for float and double.");
emit(2, " * Note that for arrays only the first and last elements are considered.");
emit(2, " *");
emit(2, " * @return success.");
emit(2, " */");
emit(2, "inline bool initTestObject();");
emit(0, "");
emit(2, "/**");
emit(2, " * Increment every variable in the object.");
emit(2, " * Works well with initTestObject() and compareTestObject().");
emit(2, " * Note that for arrays only the first and last elements are considered.");
emit(2, " *");
emit(2, " * @return success.");
emit(2, " */");
emit(2, "inline bool incTestObject();");
emit(0, "");
emit(2, "/**");
emit(2, " * Compare two instances of the same type.");
emit(2, " * Works well with initTestObject() and incTestObject().");
emit(2, " * Note that for arrays only the first and last elements are considered.");
emit(2, " *");
emit(2, " * @param[in] other The object to compare.");
emit(2, " * @return true if all elements are equal, false otherwise.");
emit(2, " */");
emit(2, "inline bool compareTestObject(%s const & other) const;", sn);
emit(0, "");
emit(2, "/**");
emit(2, " * Retrieve the 64-bit fingerprint identifying the structure of the message.");
emit(2, " * Note that the fingerprint is the same for all instances of the same");
emit(2, " * message type, and is a fingerprint on the message type definition, not on");
emit(2, " * the message contents.");
emit(2, " */");
emit(2, "inline static int64_t getHash();");
emit(0, "");
emit(2, "/**");
emit(2, " * Returns \"%s\"", ls->structname->shortname);
emit(2, " */");
emit(2, "inline static const char* getTypeName();");
emit(0, "");
emit(2, "// LCM support functions. Users should not call these");
emit(2, "inline int _encodeNoHash(void *buf, int offset, int maxlen) const;");
emit(2, "inline int _getEncodedSizeNoHash() const;");
emit(2, "inline int _decodeNoHash(const void *buf, int offset, int maxlen);");
emit(2, "inline static uint64_t _computeHash(const __lcm_hash_ptr *p);");
emit(0, "};");
emit(0, "");
free(tn_);
}
int getopt_get_int(getopt_t *getopt, const char *lname)
{
const char *v = getopt_get_string(getopt, lname);
assert(v != NULL);
return atoi(v);
}
