static void match_func_end(struct symbol *sym)
{
if (__inline_fn)
return;
if (!is_reachable() && !returned)
sm_info("info: add to no_return_funcs");
returned = 0;
}
static void match_end_func(struct symbol *sym)
{
if (__inline_fn)
return;
if (returns_new_stuff && !returns_old_stuff)
sm_info("allocation func");
free_trackers_and_list(&allocated);
returns_new_stuff = 0;
returns_old_stuff = 0;
}
static void print_returns_held(struct expression *expr)
{
struct sm_state *sm;
if (!option_info)
return;
sm = get_sm_state_expr(my_id, expr);
if (!sm)
return;
if (slist_has_state(sm->possible, &held))
sm_info("returned dev is held.");
}
int main(int argc, const char*argv[]) {
sm_set_program_name(argv[0]);
struct sm_params params;
struct sm_result result;
struct option* ops = options_allocate(100);
options_string(ops, "in", &p.file_in, "stdin", "Input file ");
options_string(ops, "out", &p.file_out, "stdout", "Output file ");
options_string(ops, "out_stats", &p.file_out_stats, "", "Output file (stats) ");
options_string(ops, "file_jj", &p.file_jj, "",
"File for journaling -- if left empty, journal not open.");
options_int(ops, "algo", &p.algo, 0, "Which algorithm to use (0:(pl)ICP 1:gpm-stripped 2:HSM) ");
options_int(ops, "debug", &p.debug, 0, "Shows debug information");
options_int(ops, "recover_from_error", &p.recover_from_error, 0, "If true, tries to recover from an ICP matching error");
p.format = 0;
/* options_int(ops, "format", &p.format, 0,
"Output format (0: log in JSON format, 1: log in Carmen format (not implemented))");*/
sm_options(¶ms, ops);
if(!options_parse_args(ops, argc, argv)) {
fprintf(stderr, "\n\nUsage:\n");
options_print_help(ops, stderr);
return -1;
}
sm_debug_write(p.debug);
/* Open input and output files */
FILE * file_in = open_file_for_reading(p.file_in);
if(!file_in) return -1;
FILE * file_out = open_file_for_writing(p.file_out);
if(!file_out) return -1;
if(strcmp(p.file_jj, "")) {
FILE * jj = open_file_for_writing(p.file_jj);
if(!jj) return -1;
jj_set_stream(jj);
}
FILE * file_out_stats = 0;
if(strcmp(p.file_out_stats, "")) {
file_out_stats = open_file_for_writing(p.file_out_stats);
if(!file_out_stats) return -1;
}
/* Read first scan */
LDP laser_ref;
if(!(laser_ref = ld_read_smart(file_in))) {
sm_error("Could not read first scan.\n");
return -1;
}
if(!ld_valid_fields(laser_ref)) {
sm_error("Invalid laser data in first scan.\n");
return -2;
}
/* For the first scan, set estimate = odometry */
copy_d(laser_ref->odometry, 3, laser_ref->estimate);
spit(laser_ref, file_out);
int count=-1;
LDP laser_sens;
while( (laser_sens = ld_read_smart(file_in)) ) {
count++;
if(!ld_valid_fields(laser_sens)) {
sm_error("Invalid laser data in (#%d in file).\n", count);
return -(count+2);
}
params.laser_ref = laser_ref;
params.laser_sens = laser_sens;
/* Set first guess as the difference in odometry */
if( any_nan(params.laser_ref->odometry,3) ||
any_nan(params.laser_sens->odometry,3) ) {
sm_error("The 'odometry' field is set to NaN so I don't know how to get an initial guess. I usually use the difference in the odometry fields to obtain the initial guess.\n");
sm_error(" laser_ref->odometry = %s \n", friendly_pose(params.laser_ref->odometry) );
sm_error(" laser_sens->odometry = %s \n", friendly_pose(params.laser_sens->odometry) );
sm_error(" I will quit it here. \n");
return -3;
}
double odometry[3];
pose_diff_d(laser_sens->odometry, laser_ref->odometry, odometry);
double ominus_laser[3], temp[3];
ominus_d(params.laser, ominus_laser);
oplus_d(ominus_laser, odometry, temp);
oplus_d(temp, params.laser, params.first_guess);
/* Do the actual work */
switch(p.algo) {
case(0):
sm_icp(¶ms, &result); break;
case(1):
sm_gpm(¶ms, &result); break;
case(2):
sm_hsm(¶ms, &result); break;
default:
sm_error("Unknown algorithm to run: %d.\n",p.algo);
return -1;
}
if(!result.valid){
if(p.recover_from_error) {
sm_info("One ICP matching failed. Because you passed -recover_from_error, I will try to recover."
" Note, however, that this might not be good in some cases. \n");
sm_info("The recover is that the displacement is set to 0. No result stats is output. \n");
/* For the first scan, set estimate = odometry */
copy_d(laser_ref->estimate, 3, laser_sens->estimate);
ld_free(laser_ref); laser_ref = laser_sens;
} else {
sm_error("One ICP matching failed. Because I process recursively, I will stop here.\n");
sm_error("Use the option -recover_from_error if you want to try to recover.\n");
ld_free(laser_ref);
return 2;
}
} else {
/* Add the result to the previous estimate */
oplus_d(laser_ref->estimate, result.x, laser_sens->estimate);
/* Write the corrected log */
spit(laser_sens, file_out);
/* Write the statistics (if required) */
if(file_out_stats) {
JO jo = result_to_json(¶ms, &result);
fputs(jo_to_string(jo), file_out_stats);
fputs("\n", file_out_stats);
jo_free(jo);
}
ld_free(laser_ref); laser_ref = laser_sens;
}
}
ld_free(laser_ref);
return 0;
}
LDP ld_from_carmen_string(const char*line) {
if(0 != strncmp(line, carmen_prefix, strlen(carmen_prefix))) {
sm_error("This is not a Carmen line: \n-> %s\n", line);
return 0;
}
size_t cur = strlen(carmen_prefix);
int nrays=-1;
if(read_next_integer(line, &cur, &nrays)) {
sm_error("Could not get number of rays.\n");
goto error;
}
LDP ld = ld_alloc_new(nrays);
double fov = M_PI;
double min_reading = 0;
double max_reading = 80;
if(nrays == 769) {
min_reading = 0.001;
max_reading = 4;
fov = deg2rad(270.0);
static int print = 0;
if(!print) { print = 1;
sm_info("Assuming that 769 rays is an Hokuyo "
"with fov = %f deg, min_reading = %f m, max_reading = %fm\n",
rad2deg(fov), min_reading, max_reading);
}
}
ld->min_theta = -fov/2;
ld->max_theta = +fov/2;
int on_error = 0;
int i;
for(i=0;i<nrays;i++) {
double reading;
if(read_next_double(line,&cur,&reading)) {
sm_error("Could not read ray #%d / %d, \n", i, nrays);
on_error = 1;
break;
}
ld->valid[i] = (reading > min_reading) && (reading < max_reading);
ld->readings[i] = ld->valid[i] ? reading : NAN;
ld->theta[i] = ld->min_theta + i *
(ld->max_theta-ld->min_theta) / (ld->nrays-1);
/* bad hokuyo!! */
if(nrays == 769) {
if(i>725 || i<44) {
ld->valid[i] = 0;
ld->readings[i] = NAN;
}
}
}
if(on_error) goto error;
if(read_next_double(line,&cur,ld->estimate+0)) goto error;
if(read_next_double(line,&cur,ld->estimate+1)) goto error;
if(read_next_double(line,&cur,ld->estimate+2)) goto error;
if(read_next_double(line,&cur,ld->odometry+0)) goto error;
if(read_next_double(line,&cur,ld->odometry+1)) goto error;
if(read_next_double(line,&cur,ld->odometry+2)) goto error;
/* Following: ipc_timestamp hostname timestamp */
/* Two csm_options:
double string double:
the first is timestamp in seconds, the second is discarded
int string int:
the first is sec, the second is usec
*/
static int warn_format = 1;
int inc; int sec=-1, usec=-1;
int res = sscanf(line + cur, "%d %s %d%n", &sec, ld->hostname, &usec, &inc);
if(3 == res) {
ld->tv.tv_sec = sec;
ld->tv.tv_usec = usec;
if(warn_format)
sm_info("Reading timestamp as 'sec hostname usec'.\n");
} else {
double v1=-1, v2=-1;
res = sscanf(line + cur, "%lf %s %lf%n", &v1, ld->hostname, &v2, &inc);
if(3 == res) {
ld->tv.tv_sec = (int) floor(v1);
ld->tv.tv_usec = floor( (v1 - floor(v1)) * 1e6 );
if(warn_format)
sm_info("Reading timestamp as doubles (discarding second one).\n");
} else {
ld->tv.tv_sec = 0;
ld->tv.tv_usec = 0;
if(warn_format)
sm_info("I could not read timestamp+hostname; ignoring (I will warn only once for this).\n");
}
}
warn_format = 0;
fprintf(stderr, "l");
return ld;
error:
printf("Malformed line: '%s'\nat cur = %d\n\t-> '%s'\n", line,(int)cur,line+cur);
return 0;
}
