int
main(int argc, char **argv)
{
FILE *flist = fopen(argv[2], "r");
carmen_FILE *fin;
carmen_FILE *fout;
char fname_in[2000];
char fname_out[2000];
char map_name[2000];
if (argc != 5)
{
printf("Usage:\n convert_maps_from_float_to_double <number_of_maps> <file_with_map_list.txt> <path_of_float_maps> <path_of_double_maps>\n");
exit(0);
}
int number_of_maps = atoi(argv[1]);
for (int i = 0; i < number_of_maps; i++)
{
fscanf(flist, "%s", map_name);
printf("%s\n", map_name);
sprintf(fname_in, "%s/%s", argv[3], map_name);
sprintf(fname_out, "%s/%s", argv[4], map_name);
fin = carmen_fopen(fname_in, "r");
fout = carmen_fopen(fname_out, "w");
carmen_map_read_gridmap(fin, fout);
carmen_fclose(fin);
carmen_fclose(fout);
}
}
static void carmen_map_read_gridmap(carmen_FILE *fin, carmen_FILE *fout)
{
int size_x, size_y;
float resolution;
float *complete_map = NULL;
carmen_map_t map;
int chunk_type, chunk_size;
char chunk_description[12];
if(carmen_map_advance_to_chunk(fin, CARMEN_MAP_GRIDMAP_CHUNK) < 0) {
fprintf(stderr, "Error: Could not find a gridmap chunk.\n");
fprintf(stderr, " This file is probably not a map file.\n");
carmen_fclose(fin);
}
chunk_type = carmen_fgetc(fin);
carmen_fread(&chunk_size, sizeof(int), 1, fin);
carmen_fread(chunk_description, 10, 1, fin);
chunk_description[10] = '\0';
if(CARMEN_MAP_CHUNK_IS_NAMED(chunk_type)) {
if(read_string(NULL, -1, fin) < 0) {
carmen_warn("Error: Unexpected EOF.\n");
carmen_fclose(fin);
}
}
carmen_fread(&size_x, sizeof(int), 1, fin);
carmen_fread(&size_y, sizeof(int), 1, fin);
carmen_fread(&resolution, sizeof(float), 1, fin);
printf("%d %d %lf\n", size_x, size_y, resolution);
carmen_grid_mapping_create_new_map(&map, size_x, size_y, resolution);
complete_map = (float *)calloc(map.config.x_size * map.config.y_size, sizeof(float));
carmen_fread(complete_map, sizeof(float) * size_x * size_y, 1, fin);
for (int i = 0; i < map.config.x_size * map.config.y_size; i++)
{
map.complete_map[i] = complete_map[i];
}
carmen_map_write_all(fout, map.map,
map.config.x_size,
map.config.y_size,
map.config.resolution,
(char *)"", (char *)"", (char *)"", (char *)"Generated by LCAD-UFES",
NULL, 0, NULL, 0, NULL, 0);
free(complete_map);
free(map.complete_map);
free(map.map);
}
static void strip(int argc, char *argv[])
{
char *input_filename, *output_filename, *chunk_type;
carmen_FILE *fp_in, *fp_out;
int next_arg;
int force;
next_arg = handle_options(argc, argv, &force);
next_arg++;
if(argc - next_arg != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s strip <in map filename> "
"<out map filename> <chunk type>\n\n", argv[0]);
}
input_filename = check_mapfile(argv[next_arg]);
output_filename = check_output(argv[next_arg+1], force);
chunk_type = argv[next_arg+2];
fp_in = carmen_fopen(input_filename, "r");
if(fp_in == NULL)
carmen_die_syserror("Could not open %s for reading", input_filename);
fp_out = carmen_fopen(output_filename, "w");
if(fp_out == NULL)
carmen_die_syserror("Could not open %s for writing", output_filename);
if (carmen_strcasecmp(chunk_type, "laserscans") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_LASERSCANS_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "gridmap") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_GRIDMAP_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "offlimits") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_OFFLIMITS_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "expected") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_EXPECTED_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "places") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
carmen_fclose(fp_in);
carmen_fclose(fp_out);
}
void shutdown_module(int sig)
{
if(sig == SIGINT) {
carmen_fclose(outfile);
carmen_ipc_disconnect();
fprintf(stderr, "\nDisconnecting.\n");
exit(0);
}
}
bool LogFile::load(const char* filename, bool verbose) {
char line[100000];
carmen_FILE *logfile = NULL;
carmen_logfile_index_p logfile_index = NULL;
logfile = carmen_fopen(filename, "r");
if(logfile == NULL) {
if (verbose)
carmen_warn("Error: could not open file %s for reading.\n", filename);
return false;
}
/* index the logfile */
logfile_index = carmen_logfile_index_messages(logfile);
for(int i = 0; i < logfile_index->num_messages; i++) {
/* read i-th line */
carmen_logfile_read_line(logfile_index, logfile, i, 100000, line);
/* create messages */
if(strncmp(line, "ODOM ", 5) == 0) {
push_back(new OdometryMessage(line));
}
else if(strncmp(line, "RAWLASER", 8) == 0) {
push_back(new LaserMessage(line));
}
else if(strncmp(line, "ROBOTLASER", 10) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "FLASER ", 7) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "RLASER ", 7) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "TRUEPOS ", 8) == 0) {
push_back(new TrueposMessage(line));
}
else if(strncmp(line, "IMU ", 4) == 0) {
push_back(new IMUMessage(line));
}
else if(strncmp(line, "SCANMARK ", 9) == 0) {
push_back(new ScanmarkMessage(line));
}
else if(strncmp(line, "POSITIONLASER ", 14) == 0) {
push_back(new LaserposMessage(line));
}
else if(strlen(line) > 1) {
push_back(new UnknownMessage(line));
}
}
carmen_fclose(logfile);
return true;
}
void
save_local_map()
{
carmen_FILE *fp;
fp = carmen_fopen("local.map", "w");
carmen_map_write_all(fp, local_map.map,
local_map.config.x_size,
local_map.config.y_size,
local_map.config.resolution,
(char *) "", (char *) "", (char *) "", (char *) "Generated by big_map",
NULL, 0, NULL, 0, NULL, 0);
carmen_fclose(fp);
}
static void tomap(int argc, char *argv[])
{
int next_arg;
int force;
double resolution;
char *input_filename, *output_filename;
carmen_map_t *map;
carmen_FILE *out_fp;
char buf[1024];
gtk_init (&argc, &argv);
next_arg = handle_options(argc, argv, &force);
if(argc - next_arg != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s tomap <resolution> <ppm filename> "
"<map filename>\n\n", argv[0]);
}
resolution = (double)atof(argv[next_arg+1]);
if (resolution == 0)
carmen_die("%s translated to a resolution of 0.\n"
"A positive, non-zero resolution is required.\n",
argv[next_arg+1]);
input_filename = argv[next_arg+2];
if(!carmen_file_exists(input_filename))
carmen_die("Image file %s does not exist.\n", input_filename);
output_filename = check_output(argv[next_arg+3], force);
map = carmen_map_imagefile_to_map(input_filename, resolution);
out_fp = carmen_fopen(output_filename, "w");
if (carmen_map_write_id(out_fp) < 0)
carmen_die_syserror("Couldn't write map id to %s", output_filename);
sprintf(buf, "Created from %s", input_filename);
if (carmen_map_write_creator_chunk(out_fp, "img_to_map", buf) < 0)
carmen_die_syserror("Couldn't write creator chunk to %s", output_filename);
if (carmen_map_write_gridmap_chunk(out_fp, map->map, map->config.x_size,
map->config.y_size,
map->config.resolution) < 0)
carmen_die_syserror("Couldn't write gridmap chunk to %s", output_filename);
carmen_fclose(out_fp);
}
int
main(int argc, char **argv)
{
char *in_filename, *out_filename;
carmen_FILE *out_fp;
char buf[1024];
carmen_map_p map;
if (argc != 3)
carmen_die("Usage: %s <bee_filename> <carmen_filename>\n", argv[0]);
in_filename = argv[1];
out_filename = argv[2];
if (carmen_file_exists(out_filename))
{
printf("Overwrite %s? ", out_filename);
scanf("%s", buf);
if (tolower(buf[0]) != 'y')
exit(0);
}
map = read_beesoft_map(in_filename);
out_fp = carmen_fopen(out_filename, "w");
if (out_fp == NULL)
carmen_die("Couldn't open %s for writing : %s\n", out_filename,
strerror(errno));
if (carmen_map_write_id(out_fp) < 0)
carmen_die_syserror("Couldn't write map id to %s", out_filename);
sprintf(buf, "Created from %s", out_filename);
if (carmen_map_write_creator_chunk(out_fp, "bee2carmen", buf) < 0)
carmen_die_syserror("Couldn't write creator chunk to %s", out_filename);
if (carmen_map_write_gridmap_chunk(out_fp, map->map, map->config.x_size,
map->config.y_size,
map->config.resolution) < 0)
carmen_die_syserror("Couldn't write gridmap chunk to %s", out_filename);
carmen_fclose(out_fp);
return 0;
}
int main(int argc, char **argv)
{
char filename[1024];
char key;
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
if(argc < 2)
carmen_die("usage: %s <logfile>\n", argv[0]);
sprintf(filename, "%s", argv[1]);
outfile = carmen_fopen(filename, "r");
if (outfile != NULL) {
fprintf(stderr, "Overwrite %s? ", filename);
scanf("%c", &key);
if (toupper(key) != 'Y')
exit(-1);
carmen_fclose(outfile);
}
outfile = carmen_fopen(filename, "w");
if(outfile == NULL)
carmen_die("Error: Could not open file %s for writing.\n", filename);
carmen_logwrite_write_header(outfile);
carmen_robot_subscribe_frontlaser_message(NULL,
(carmen_handler_t)robot_frontlaser_handler,
CARMEN_SUBSCRIBE_ALL);
localize_msg = (carmen_localize_globalpos_message *)
calloc(1, sizeof(carmen_localize_globalpos_message));
carmen_test_alloc(localize_msg);
carmen_localize_subscribe_globalpos_message(localize_msg, NULL, CARMEN_SUBSCRIBE_ALL);
logger_starttime = carmen_get_time();
signal(SIGINT, shutdown_module);
carmen_ipc_dispatch();
return 0;
}
bool LogFile::save(const char* filename, bool verbose) const {
carmen_FILE *logfile = NULL;
logfile = carmen_fopen(filename, "w");
if(logfile == NULL) {
if (verbose)
carmen_warn("Error: could not open file %s for writing.\n", filename);
return false;
}
double logger_starttime = -1.0;
for (Collection::const_iterator it = begin();
it != end(); ++it) {
if (logger_starttime < 0)
logger_starttime = (*it)->getTimestamp();
(*it)->save(logfile, (*it)->getTimestamp() - logger_starttime) ;
}
carmen_fclose(logfile);
return true;
}
int main(int argc, char **argv)
{
char filename[1024];
char key;
int num_scanned;
/* initialize connection to IPC network */
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
/* open logfile, check if file overwrites something */
if(argc < 2)
carmen_die("usage: %s <logfile>\n", argv[0]);
sprintf(filename, "%s", argv[1]);
outfile = carmen_fopen(filename, "r");
if (outfile != NULL) {
fprintf(stderr, "Overwrite %s? ", filename);
num_scanned = scanf("%c", &key);
if (toupper(key) != 'Y')
exit(-1);
carmen_fclose(outfile);
}
outfile = carmen_fopen(filename, "w");
if(outfile == NULL)
carmen_die("Error: Could not open file %s for writing.\n", filename);
carmen_logwrite_write_header(outfile);
get_logger_params(argc, argv);
if ( !(log_odometry && log_laser && log_robot_laser ) )
carmen_warn("\nWARNING: You are neither logging laser nor odometry messages!\n");
if (log_params)
get_all_params();
register_ipc_messages();
if (log_odometry)
carmen_base_subscribe_odometry_message(NULL, (carmen_handler_t)
base_odometry_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_sonars)
carmen_base_subscribe_sonar_message(NULL, (carmen_handler_t)
base_sonar_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_bumpers)
carmen_base_subscribe_bumper_message(NULL, (carmen_handler_t)
base_bumper_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_arm)
carmen_arm_subscribe_state_message(NULL, (carmen_handler_t)
arm_state_handler,
CARMEN_SUBSCRIBE_ALL);
if (log_pantilt) {
carmen_pantilt_subscribe_scanmark_message (NULL, (carmen_handler_t)
pantilt_scanmark_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_pantilt_subscribe_laserpos_message (NULL, (carmen_handler_t)
pantilt_laserpos_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_robot_laser) {
carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)
robot_frontlaser_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)
robot_rearlaser_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_laser) {
carmen_laser_subscribe_laser1_message(NULL, (carmen_handler_t)
laser_laser1_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser2_message(NULL, (carmen_handler_t)
laser_laser2_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser3_message(NULL, (carmen_handler_t)
laser_laser3_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser4_message(NULL, (carmen_handler_t)
laser_laser4_handler,
CARMEN_SUBSCRIBE_ALL);
carmen_laser_subscribe_laser5_message(NULL, (carmen_handler_t)
laser_laser5_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_localize) {
carmen_localize_subscribe_globalpos_message(NULL, (carmen_handler_t)
localize_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_simulator) {
carmen_simulator_subscribe_truepos_message(NULL, (carmen_handler_t)
carmen_simulator_truepos_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_imu) {
carmen_imu_subscribe_imu_message(NULL, (carmen_handler_t)
imu_handler,
CARMEN_SUBSCRIBE_ALL);
}
if (log_gps) {
carmen_gps_subscribe_nmea_message( NULL,
(carmen_handler_t) ipc_gps_gpgga_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_gps_subscribe_nmea_rmc_message( NULL,
(carmen_handler_t) ipc_gps_gprmc_handler,
CARMEN_SUBSCRIBE_ALL );
}
if (log_motioncmds) {
carmen_robot_subscribe_vector_move_message( NULL,
(carmen_handler_t) robot_vector_move_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_robot_subscribe_follow_trajectory_message( NULL,
(carmen_handler_t) robot_follow_trajectory_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_robot_subscribe_velocity_message( NULL,
(carmen_handler_t) robot_velocity_handler,
CARMEN_SUBSCRIBE_ALL );
carmen_base_subscribe_velocity_message( NULL,
(carmen_handler_t) base_velocity_handler,
CARMEN_SUBSCRIBE_ALL );
}
carmen_logger_subscribe_comment_message( NULL,
(carmen_handler_t) logger_comment_handler,
CARMEN_SUBSCRIBE_ALL );
signal(SIGINT, shutdown_module);
logger_starttime = carmen_get_time();
carmen_ipc_dispatch();
return 0;
}
int
main( int argc, char *argv[] )
{
carmen_point_t pos, corrpos, odo;
double time;
int i;
int scancnt;
carmen_move_t pos_move = {0.0, 0.0, 0.0};
carmen_move_t corr_move = {0.0, 0.0, 0.0};
carmen_point_t old_pos = {0.0, 0.0, 0.0};
carmen_point_t old_corrpos = {0.0, 0.0, 0.0};
int which_laser = 0;
carmen_FILE stdout_carmen;
stdout_carmen.compressed = 0;
stdout_carmen.fp = stdout;
if (argc < 2) {
print_usage();
exit(1);
}
scancnt=0;
for (i=1; i < argc-1; i++) {
if (!strcmp(argv[i], "-f") || !strcmp(argv[i], "--flaser"))
which_laser = 1;
if (!strcmp(argv[i], "-r") || !strcmp(argv[i], "--robotlaser"))
which_laser = 2;
}
carmen_ipc_initialize(argc, argv);
vascocore_init(argc, argv);
/* file handler */
carmen_FILE *logfile = NULL;
/* log file index structure */
carmen_logfile_index_p logfile_index = NULL;
/* open logfile for reading */
logfile = carmen_fopen(argv[argc-1], "r");
if(logfile == NULL)
carmen_die("Error: could not open file %s for reading.\n", argv[1]);
/* create index structure */
logfile_index = carmen_logfile_index_messages(logfile);
/* buffer for reading the lines */
const int max_line_length=9999;
char line[max_line_length+1];
/* used to read the messages. erase stucture before! */
carmen_robot_laser_message l;
carmen_erase_structure(&l, sizeof(carmen_robot_laser_message) );
/* iterate over all entries in the logfile */
while (!carmen_logfile_eof(logfile_index)) {
int correct_msg = 1;
/* read the line from the file */
carmen_logfile_read_next_line(logfile_index,
logfile,
max_line_length,
line);
/* what kind of message it this? */
if (which_laser != 1 && strncmp(line, "ROBOTLASER1 ", 12) == 0) {
/* convert the string using the corresponding read function */
char* next = carmen_string_to_robot_laser_message(line, &l);
time = atof( strtok( next, " ") );
}
else if (which_laser != 2 && strncmp(line, "FLASER ", 7) == 0) {
/* convert the string using the corresponding read function */
char* next = carmen_string_to_robot_laser_message_orig(line, &l);
time = atof( strtok( next, " ") );
}
else {
correct_msg = 0;
fputs(line, stdout);
}
if (correct_msg) {
odo = l.robot_pose;
pos = l.laser_pose;
/* using the new scan match function that takes robot laser messages...
carmen_laser_laser_message scan;
carmen_erase_structure(&scan, sizeof(carmen_laser_laser_message) );
scan.timestamp = l.timestamp;
scan.config = l.config;
scan.num_readings = l.num_readings;
scan.num_remissions = l.num_remissions;
scan.range = l.range;
scan.remission = l.remission;
scan.host = l.host;
corrpos = vascocore_scan_match( scan, pos );*/
corrpos = vascocore_scan_match_robot( l );
scancnt++;
corr_move = carmen_move_between_points( old_corrpos, corrpos );
pos_move = carmen_move_between_points( old_pos, pos );
fprintf( stderr, "expected movement %.6f m %.6f m %.6f degree\n",
corr_move.forward, corr_move.sideward,
rad2deg(corr_move.rotation) );
fprintf( stderr, "corrected movement %.6f m %.6f m %.6f degree\n\n",
pos_move.forward, pos_move.sideward,
rad2deg(pos_move.rotation) );
l.laser_pose = corrpos;
carmen_logwrite_write_robot_laser(&l,1,&stdout_carmen, time);
fflush(stdout);
old_pos = pos;
old_corrpos = corrpos;
}
}
/* close the file */
carmen_fclose(logfile);
fprintf(stderr, "\ndone, corrected %d messages!\n", scancnt);
/* free index structure */
carmen_logfile_free_index(&logfile_index);
return(0);
}
int main(int argc, char **argv)
{
FILE *fp;
carmen_FILE *outfile;
int m, d, y, h, min, temp1, temp2, i;
float s;
// int type_id;
double timestamp, first_timestamp = 0;
double last_front_laser_timestamp, last_odometry_timestamp;
int first = 1;
char key;
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
if(argc < 3) {
fprintf(stderr, "Error: wrong number of arguments.\n");
fprintf(stderr, "Usage: %s laserint-logname logname\n", argv[0]);
exit(1);
}
fp = fopen(argv[1], "r");
if(fp == NULL) {
fprintf(stderr, "Error: could not open file %s for reading.\n", argv[1]);
exit(1);
}
outfile = carmen_fopen(argv[2], "r");
if (outfile != NULL) {
fprintf(stderr, "Overwrite %s? ", argv[2]);
scanf("%c", &key);
if (toupper(key) != 'Y')
exit(-1);
carmen_fclose(outfile);
}
outfile = carmen_fopen(argv[2], "w");
if(outfile == NULL) {
fprintf(stderr, "Error: could not open file %s for writing.\n", argv[2]);
exit(1);
}
front_laser.num_readings = 180;
front_laser.range = (float *)calloc(180, sizeof(float));
carmen_test_alloc(front_laser.range);
front_laser.host = carmen_get_host();
odometry.host = carmen_get_host();
carmen_logwrite_write_header(outfile);
carmen_logwrite_write_robot_name("beesoft", outfile);
get_all_params(outfile);
while(!feof(fp)) {
fgets(line, LINE_SIZE, fp);
if(strncmp(line, "@SENS", 5) == 0) {
fgets(line2, 7, fp);
if(strncmp(line2, "#LASER", 6) == 0) {
last_front_laser_timestamp = front_laser_timestamp;
sscanf(line, "@SENS %d-%d-%d %d:%d:%f\n", &m, &d, &y, &h, &min, &s);
timestamp = h * 3600.0 + min * 60.0 + s;
fscanf(fp, " %d %d: ", &temp1, &temp2);
for(i = 0; i < 180; i++) {
fscanf(fp, "%d", &temp1);
front_laser.range[i] = temp1/100.0;
}
front_laser.laser_pose.x = odometry.x;
front_laser.laser_pose.y = odometry.y;
front_laser.laser_pose.theta =
odometry.theta;
front_laser.robot_pose.x = odometry.x;
front_laser.robot_pose.y = odometry.y;
front_laser.robot_pose.theta =
odometry.theta;
if(first) {
first_timestamp = timestamp;
first = 0;
}
front_laser_timestamp = timestamp - first_timestamp;
if(current_front_laser > 0 &&
front_laser_timestamp < last_front_laser_timestamp)
front_laser_timestamp = last_front_laser_timestamp;
// type_id = ROBOT_FRONTLASER_ID;
front_laser.timestamp = front_laser_timestamp;
carmen_logwrite_write_robot_laser(&front_laser, 1, outfile, front_laser_timestamp);
current_front_laser++;
}
else if(strncmp(line2, "#ROBOT", 6) == 0) {
last_odometry_timestamp = odometry_timestamp;
sscanf(line, "@SENS %d-%d-%d %d:%d:%f\n", &m, &d, &y, &h, &min, &s);
timestamp = h * 3600.0 + min * 60.0 + s;
fscanf(fp, " %lf %lf %lf", &odometry.x, &odometry.y,
&odometry.theta);
odometry.x /= 100.0;
odometry.y /= 100.0;
odometry.theta = carmen_normalize_theta(carmen_degrees_to_radians(odometry.theta));
if(first) {
first_timestamp = timestamp;
first = 0;
}
odometry_timestamp = timestamp - first_timestamp;
if(current_odometry > 0 &&
odometry_timestamp < last_odometry_timestamp)
odometry_timestamp = last_odometry_timestamp;
// type_id = ODOM_ID;
carmen_logwrite_write_odometry(&odometry, outfile, odometry_timestamp);
current_odometry++;
}
fgets(line2, LINE_SIZE, fp);
}
}
fclose(fp);
carmen_fclose(outfile);
return 0;
}
//
// This calls the procedures in the other files which do all the real work.
// If you wanted to not use hierarchical SLAM, you could remove all references here to High*, and make
// certain to set LOW_DURATION in low.h to some incredibly high number.
//
void *Slam(void * /*arg unused */)
{
TPath *path, *trashPath;
TSenseLog *obs, *trashObs;
double oldmC_D = 0;
double oldmC_T = 0;
double oldvC_D = 0;
double oldvC_T = 0;
double oldmD_D = 0;
double oldmD_T = 0;
double oldvD_D = 0;
double oldvD_T = 0;
double oldmT_D = 0;
double oldmT_T = 0;
double oldvT_D = 0;
double oldvT_T = 0;
outFile = fopen("motionModel.txt", "w");
fprintf(outFile, "This file lists the motion model parameters in the following order : \n");
fprintf(outFile, " - mean value from observed translation\n");
fprintf(outFile, " - mean value from observed rotation\n");
fprintf(outFile, " - variance contributed from observed translation\n");
fprintf(outFile, " - variance contributed from observed rotation\n");
fprintf(outFile, "\n");
fprintf(outFile, "------------Intermediate Models------------------\n");
fprintf(outFile, " m_dist m_turn v_dist v_turn\n");
fprintf(outFile, "C: %.4f %.4f %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "D: %.4f %.4f %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "T: %.4f %.4f %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T);
fprintf(outFile, "\n");
fclose(outFile);
while ((fabs(oldmC_D-meanC_D) > 0.01)||(fabs(oldmC_T-meanC_T) > 0.03)||(fabs(oldvC_D-varC_D) > 0.01)||(fabs(oldvC_T-varC_T) > 0.1) ||
(fabs(oldmD_D-meanD_D) > 0.01)||(fabs(oldmD_T-meanD_T) > 0.03)||(fabs(oldvD_D-varD_D) > 0.01)||(fabs(oldvD_T-varD_T) > 0.1) ||
(fabs(oldmT_D-meanT_D) > 0.03)||(fabs(oldmT_T-meanT_T) > 0.01)||(fabs(oldvT_D-varT_D) > 0.03)||(fabs(oldvT_T-varT_T) > 0.01)) {
readFile = carmen_fopen(PLAYBACK, "r");
if(readFile == NULL)
carmen_die("Error: could not open file %s for reading.\n", PLAYBACK);
logfile_index = carmen_logfile_index_messages(readFile);
InitLowSlam();
LowSlam(&path, &obs);
carmen_fclose(readFile);
oldmC_D = meanC_D;
oldmC_T = meanC_T;
oldvC_D = varC_D;
oldvC_T = varC_T;
oldmD_D = meanD_D;
oldmD_T = meanD_T;
oldvD_D = varD_D;
oldvD_T = varD_T;
oldmT_D = meanT_D;
oldmT_T = meanT_T;
oldvT_D = varT_D;
oldvT_T = varT_T;
outFile = fopen("motionModel.txt", "a");
Learn(path);
fclose(outFile);
// Get rid of the path and log of observations
while (path != NULL) {
trashPath = path;
path = path->next;
free(trashPath);
}
while (obs != NULL) {
trashObs = obs;
obs = obs->next;
free(trashObs);
}
}
outFile = fopen("motionModel.txt", "a");
fprintf(outFile, "\n");
fprintf(outFile, "------------Final Model------------------\n");
fprintf(outFile, " m_dist m_turn v_dist v_turn\n");
fprintf(outFile, "C: %.4f %.4f %.4f %.4f\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "D: %.4f %.4f %.4f %.4f\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "T: %.4f %.4f %.4f %.4f\n", meanT_D, meanT_T, varT_D, varT_T);
fprintf(outFile, "\n");
fprintf(outFile, "#define meanC_D %.4f\n#define meanC_T %.4f\n#define varC_D %.4f\n#define varC_T %.4f\n\n", meanC_D, meanC_T, varC_D, varC_T);
fprintf(outFile, "#define meanD_D %.4f\n#define meanD_T %.4f\n#define varD_D %.4f\n#define varD_T %.4f\n\n", meanD_D, meanD_T, varD_D, varD_T);
fprintf(outFile, "#define meanT_D %.4f\n#define meanT_T %.4f\n#define varT_D %.4f\n#define varT_T %.4f\n\n", meanT_D, meanT_T, varT_D, varT_T);
fclose(outFile);
CloseLowSlam();
return NULL;
}
glv_object_p glv_object_read(char *filename) {
carmen_FILE *fp;
glv_color_t current_color;
char buffer[10000];
long int nread, log_bytes = 0;
int buffer_pos, buffer_length, offset = 0, n;
glv_object_p obj;
float *fmessage;
int done_reading = 0;
int i, line_count = 0;
float min_x, max_x, min_y, max_y, min_z, max_z;
if(strncmp(filename + strlen(filename) - 4, ".glv", 4) &&
strncmp(filename + strlen(filename) - 7, ".glv.gz", 7) &&
strncmp(filename + strlen(filename) - 5, ".pmap", 5) &&
strncmp(filename + strlen(filename) - 8, ".pmap.gz", 8))
carmen_die("Error: file name must end in .glv, .glv.gz, .pmap, or .pmap.gz\n");
fp = carmen_fopen(filename, "r");
/* compute total number of bytes in logfile */
do {
nread = carmen_fread(buffer, 1, 10000, fp);
log_bytes += nread;
} while(nread > 0);
carmen_fseek(fp, 0L, SEEK_SET);
current_color.r = 255;
current_color.g = 255;
current_color.b = 255;
obj = glv_object_init();
buffer_pos = 0;
buffer_length = carmen_fread(buffer, 1, 10000, fp);
while(!done_reading || buffer_length > buffer_pos) {
line_count++;
if(line_count % 100000 == 0)
fprintf(stderr, "\rReading glv file... (%.0f%%) ",
(offset + buffer_pos) / (float)log_bytes * 100.0);
if(buffer_length - buffer_pos < 50 && !done_reading) {
memmove(buffer, buffer + buffer_pos, buffer_length - buffer_pos);
buffer_length -= buffer_pos;
offset += buffer_pos;
buffer_pos = 0;
n = carmen_fread(buffer + buffer_length,
1, 10000 - buffer_length - 1, fp);
if(n == 0)
done_reading = 1;
else
buffer_length += n;
}
else {
if(buffer[buffer_pos] == MESSAGE_ID_COLOR) {
current_color.r = (unsigned char)buffer[buffer_pos + 1];
current_color.g = (unsigned char)buffer[buffer_pos + 2];
current_color.b = (unsigned char)buffer[buffer_pos + 3];
buffer_pos += 4;
}
else if(buffer[buffer_pos] == MESSAGE_ID_POINT) {
if(obj->num_points == obj->max_points) {
obj->max_points += 100000;
obj->point = (glv_point_p)realloc(obj->point, obj->max_points *
sizeof(glv_point_t));
carmen_test_alloc(obj->point);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->point[obj->num_points].x = fmessage[0];
obj->point[obj->num_points].y = fmessage[1];
obj->point[obj->num_points].z = fmessage[2];
obj->point[obj->num_points].c = current_color;
obj->num_points++;
buffer_pos += 13;
}
else if(buffer[buffer_pos] == MESSAGE_ID_LINE) {
if(obj->num_lines == obj->max_lines) {
obj->max_lines += 100000;
obj->line = (glv_line_p)realloc(obj->line, obj->max_lines *
sizeof(glv_line_t));
carmen_test_alloc(obj->line);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->line[obj->num_lines].p1.x = fmessage[0];
obj->line[obj->num_lines].p1.y = fmessage[1];
obj->line[obj->num_lines].p1.z = fmessage[2];
obj->line[obj->num_lines].p2.x = fmessage[3];
obj->line[obj->num_lines].p2.y = fmessage[4];
obj->line[obj->num_lines].p2.z = fmessage[5];
obj->line[obj->num_lines].c = current_color;
obj->num_lines++;
buffer_pos += 25;
}
else if(buffer[buffer_pos] == MESSAGE_ID_FACE) {
if(obj->num_faces == obj->max_faces) {
obj->max_faces += 100000;
obj->face = (glv_face_p)realloc(obj->face, obj->max_faces *
sizeof(glv_face_t));
carmen_test_alloc(obj->face);
}
fmessage = (float *)(buffer + buffer_pos + 1);
obj->face[obj->num_faces].p1.x = fmessage[0];
obj->face[obj->num_faces].p1.y = fmessage[1];
obj->face[obj->num_faces].p1.z = fmessage[2];
obj->face[obj->num_faces].p2.x = fmessage[3];
obj->face[obj->num_faces].p2.y = fmessage[4];
obj->face[obj->num_faces].p2.z = fmessage[5];
obj->face[obj->num_faces].p3.x = fmessage[6];
obj->face[obj->num_faces].p3.y = fmessage[7];
obj->face[obj->num_faces].p3.z = fmessage[8];
obj->face[obj->num_faces].c = current_color;
compute_normal(&obj->face[obj->num_faces]);
obj->num_faces++;
buffer_pos += 37;
}
}
}
min_x = 1e10;
max_x = -1e10;
min_y = 1e10;
max_y = -1e10;
min_z = 1e10;
max_z = -1e10;
for(i = 0; i < obj->num_points; i++)
adjust_extrema(obj->point[i], &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
for(i = 0; i < obj->num_lines; i++) {
adjust_extrema(obj->line[i].p1, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->line[i].p2, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
}
for(i = 0; i < obj->num_faces; i++) {
adjust_extrema(obj->face[i].p1, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->face[i].p2, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
adjust_extrema(obj->face[i].p3, &min_x, &max_x, &min_y, &max_y,
&min_z, &max_z);
}
obj->centroid.x = (min_x + max_x) / 2;
obj->centroid.y = (min_y + max_y) / 2;
obj->centroid.z = (min_z + max_z) / 2;
obj->min.x = min_x;
obj->min.y = min_y;
obj->min.z = min_z;
obj->max.x = max_x;
obj->max.y = max_y;
obj->max.z = max_z;
carmen_fclose(fp);
fprintf(stderr, "\rReading glv file... (100%%) \n");
fprintf(stderr, "%d POINTS - %d LINES - %d FACES\n", obj->num_points,
obj->num_lines, obj->num_faces);
return obj;
}
static void add_offset(int argc, char *argv[])
{
char *input_filename;
int next_arg;
int force;
char cmd[1024];
char tmp_filename[1024];
carmen_FILE *fp_in, *fp_out;
carmen_global_offset_t offset;
int system_err;
int num_args;
int response;
next_arg = handle_options(argc, argv, &force);
next_arg++;
num_args = argc - next_arg;
if(num_args != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_warn("\nUsage: %s <mapfilename> <x> <y> <theta>\n",
argv[0]);
carmen_warn(" <theta> should be given in degrees.\n");
}
input_filename = check_mapfile(argv[next_arg]);
next_arg++;
if(!carmen_map_file(input_filename))
carmen_die("Error: %s does not appear to be a valid carmen map file;\n"
"if it is gzipped, make sure it has a \".gz\" extension.\n",
input_filename);
if(carmen_map_chunk_exists(input_filename, CARMEN_MAP_GLOBAL_OFFSET_CHUNK)) {
carmen_warn("Offset chunk exists already. Replace? [y/n] ");
response = getchar();
if (tolower(response) != 'y')
exit(0);
carmen_warn("Replacing...\n");
}
offset.x = atof(argv[next_arg]);
offset.y = atof(argv[next_arg+1]);
offset.theta = carmen_degrees_to_radians(atof(argv[next_arg+2]));
fprintf(stderr, "Set (%s %s %s) to (%.2f m, %.2f m, %.2f rad)\n",
argv[next_arg], argv[next_arg+1], argv[next_arg+2],
offset.x, offset.y, offset.theta);
fp_in = carmen_fopen(input_filename, "r");
if(fp_in == NULL)
carmen_die_syserror("Error: file %s could not be opened for reading",
input_filename);
strcpy(tmp_filename, "/tmp/newmapXXXXXX");
system_err = mkstemp(tmp_filename);
if (system_err == -1)
carmen_die_syserror("I need to create a temporary file in /tmp, but I "
"can't for the\n following reason\n");
fp_out = carmen_fopen(tmp_filename, "w");
if(fp_out == NULL)
carmen_die_syserror("Error: file could not be opened for writing");
if(carmen_map_strip(fp_in, fp_out, CARMEN_MAP_GLOBAL_OFFSET_CHUNK) < 0)
carmen_die("Error: could not strip global offset from map file.\n");
if(carmen_map_write_global_offset_chunk(fp_out, &offset) < 0)
carmen_die("Error: could not write offset chunk.\n");
carmen_fclose(fp_in);
carmen_fclose(fp_out);
sprintf(cmd, "mv -f %s %s", tmp_filename, input_filename);
system_err = system(cmd);
if (system_err != 0)
carmen_die("I created a temporary file contained the map with the new "
"place name\nat %s. I tried to copy the new file onto your "
"old file, but the copy\nfailed for the following reason: %s\n"
"\n"
"You will have to copy the new file over yourself.\n",
tmp_filename, strerror(errno));
}
static void add_place(int argc, char *argv[])
{
char *input_filename;
int next_arg;
int force;
char cmd[1024];
char tmp_filename[1024];
carmen_FILE *fp_in, *fp_out;
carmen_map_placelist_t place_list;
carmen_place_p places;
int system_err;
int num_args;
next_arg = handle_options(argc, argv, &force);
next_arg++;
num_args = argc - next_arg;
if(num_args != 4 && num_args != 5 && num_args != 8) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_warn("\nUsage: %s <mapfilename> <placename> <place params>\n",
argv[0]);
carmen_warn(" 2, 3, or 6 place parameters can be given.\n");
}
input_filename = check_mapfile(argv[next_arg]);
next_arg++;
if(!carmen_map_file(input_filename))
carmen_die("Error: %s does not appear to be a valid carmen map file;\n"
"if it is gzipped, make sure it has a \".gz\" extension.\n",
input_filename);
if(!carmen_map_chunk_exists(input_filename, CARMEN_MAP_PLACES_CHUNK)) {
place_list.num_places = 1;
place_list.places = (carmen_place_p)calloc(1, sizeof(carmen_place_t));
carmen_test_alloc(place_list.places);
} else {
carmen_map_read_places_chunk(input_filename, &place_list);
place_list.num_places++;
place_list.places = (carmen_place_p)realloc
(place_list.places, sizeof(carmen_place_t) * place_list.num_places);
/* check_alloc checked */
}
carmen_test_alloc(place_list.places);
places = place_list.places;
if(num_args == 4) {
strcpy(places[place_list.num_places - 1].name, argv[next_arg]);
places[place_list.num_places - 1].type = CARMEN_NAMED_POSITION_TYPE;
places[place_list.num_places - 1].x = atof(argv[next_arg+1]);
places[place_list.num_places - 1].y = atof(argv[next_arg+2]);
}
else if(num_args == 5) {
strcpy(places[place_list.num_places - 1].name, argv[next_arg]);
places[place_list.num_places - 1].type = CARMEN_NAMED_POSE_TYPE;
places[place_list.num_places - 1].x = atof(argv[next_arg+1]);
places[place_list.num_places - 1].y = atof(argv[next_arg+2]);
places[place_list.num_places - 1].theta =
carmen_degrees_to_radians(atof(argv[next_arg+3]));
fprintf(stderr, "Set (%s %s %s) to (%f.2m, %f.2m, %f.2 rad)\n", argv[4],
argv[5], argv[6], places[place_list.num_places - 1].x,
places[place_list.num_places - 1].y,
places[place_list.num_places - 1].theta);
}
else {
strcpy(places[place_list.num_places - 1].name, argv[next_arg]);
places[place_list.num_places - 1].type = CARMEN_LOCALIZATION_INIT_TYPE;
places[place_list.num_places - 1].x = atof(argv[next_arg+1]);
places[place_list.num_places - 1].y = atof(argv[next_arg+2]);
places[place_list.num_places - 1].theta =
carmen_degrees_to_radians(atof(argv[next_arg+3]));
places[place_list.num_places - 1].x_std = atof(argv[next_arg+4]);
places[place_list.num_places - 1].y_std = atof(argv[next_arg+5]);
places[place_list.num_places - 1].theta_std =
carmen_degrees_to_radians(atof(argv[next_arg+6]));
fprintf(stderr, "Set (%s %s %s %s %s %s) to:\n(%f.2m +/- %f.2m, %f.2m "
"+/- %f.2m, %f.2 +/- %f.2 rad)\n",
argv[next_arg+1], argv[next_arg+2], argv[next_arg+3],
argv[next_arg+4], argv[next_arg+5], argv[next_arg+6],
places[place_list.num_places - 1].x,
places[place_list.num_places - 1].x_std,
places[place_list.num_places - 1].y,
places[place_list.num_places - 1].y_std,
places[place_list.num_places - 1].theta,
places[place_list.num_places - 1].theta_std);
}
fp_in = carmen_fopen(input_filename, "r");
if(fp_in == NULL)
carmen_die_syserror("Error: file %s could not be opened for reading",
input_filename);
strcpy(tmp_filename, "/tmp/newmapXXXXXX");
system_err = mkstemp(tmp_filename);
if (system_err == -1)
carmen_die_syserror("I need to create a temporary file in /tmp, but I "
"can't for the\n following reason\n");
fp_out = carmen_fopen(tmp_filename, "w");
if(fp_out == NULL)
carmen_die_syserror("Error: file could not be opened for writing");
if(carmen_map_strip(fp_in, fp_out, CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die("Error: could not strip places from map file.\n");
if(carmen_map_write_places_chunk(fp_out, place_list.places,
place_list.num_places) < 0)
carmen_die("Error: could not write places chunk.\n");
carmen_fclose(fp_in);
carmen_fclose(fp_out);
sprintf(cmd, "mv -f %s %s", tmp_filename, input_filename);
system_err = system(cmd);
if (system_err != 0)
carmen_die("I created a temporary file contained the map with the new "
"place name\nat %s. I tried to copy the new file onto your "
"old file, but the copy\nfailed for the following reason: %s\n"
"\n"
"You will have to copy the new file over yourself.\n",
tmp_filename, strerror(errno));
}
static void minimize(int argc, char *argv[])
{
char *input_filename, *output_filename;
int next_arg;
carmen_FILE *in_fp, *out_fp;
int ret_val;
carmen_map_t map;
int x_offset, y_offset;
carmen_offlimits_list_t offlimits_list;
carmen_map_placelist_t places;
int force;
int previous_num_places;
next_arg = handle_options(argc, argv, &force);
next_arg++;
if(argc - next_arg != 2) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s minimize <in map filename> <out map filename>\n\n",
argv[0]);
}
input_filename = check_mapfile(argv[next_arg]);
output_filename = check_output(argv[next_arg+1], force);
/*
* Read the gridmap, places and offlimits chunks and minimize them
*/
if (carmen_map_read_gridmap_chunk(input_filename, &map) < 0) {
carmen_die_syserror("Couldn't read GRIDMAP_CHUNK from %s", input_filename);
}
carmen_warn("Map size was %d x %d, ",map.config.x_size, map.config.y_size);
carmen_minimize_gridmap(&map, &x_offset, &y_offset);
carmen_warn("is now %d x %d (offset %d, %d)\n", map.config.x_size,
map.config.y_size, x_offset, y_offset);
ret_val = carmen_map_chunk_exists(input_filename,
CARMEN_MAP_OFFLIMITS_CHUNK);
if (ret_val > 0) {
ret_val = carmen_map_read_offlimits_chunk
(input_filename, &(offlimits_list.offlimits),
&(offlimits_list.list_length));
if (ret_val < 0)
carmen_die_syserror("Couldn't read OFFLIMITS_CHUNK in %s",
input_filename);
carmen_minimize_offlimits(&offlimits_list, x_offset*map.config.resolution,
y_offset*map.config.resolution);
} else
offlimits_list.list_length = 0;
ret_val = carmen_map_chunk_exists(input_filename, CARMEN_MAP_PLACES_CHUNK);
if (ret_val > 0) {
ret_val = carmen_map_read_places_chunk(input_filename, &places);
if (ret_val < 0)
carmen_die_syserror("Couldn't read PLACES_CHUNK in %s", input_filename);
previous_num_places = places.num_places;
carmen_minimize_places(&places, x_offset*map.config.resolution,
y_offset*map.config.resolution,
map.config.x_size*map.config.resolution,
map.config.y_size*map.config.resolution);
if (places.num_places < previous_num_places)
carmen_warn("%d place locations were dropped from the map after "
"minimization.\n", previous_num_places-places.num_places);
} else
places.num_places = 0;
/*
* Pass everything else through untouched, and then write the rotated
* chunks at the end.
*/
in_fp = carmen_fopen(input_filename, "r");
if (in_fp == NULL)
carmen_die_syserror("Couldn't open %s for reading", input_filename);
out_fp = carmen_fopen(output_filename, "w");
if (out_fp == NULL)
carmen_die_syserror("Couldn't open %s for writing", output_filename);
if (carmen_map_vstrip(in_fp, out_fp, 3, CARMEN_MAP_GRIDMAP_CHUNK,
CARMEN_MAP_OFFLIMITS_CHUNK,
CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die_syserror("Couldn't strip map to %s", output_filename);
if (carmen_map_write_gridmap_chunk(out_fp, map.map, map.config.x_size,
map.config.y_size,
map.config.resolution) < 0)
carmen_die_syserror("Couldn't write gridmap to %s", output_filename);
if (offlimits_list.list_length > 0) {
if (carmen_map_write_offlimits_chunk(out_fp, offlimits_list.offlimits,
offlimits_list.list_length) < 0)
carmen_die_syserror("Couldn't write offlimits list to %s",
output_filename);
}
if (places.num_places > 0) {
if (carmen_map_write_places_chunk(out_fp, places.places,
places.num_places) < 0)
carmen_die_syserror("Couldn't write places list to %s", output_filename);
}
carmen_fclose(in_fp);
carmen_fclose(out_fp);
}
static void rotate(int argc, char *argv[])
{
int force;
char *input_filename, *output_filename;
int next_arg;
carmen_FILE *in_fp, *out_fp;
int ret_val;
carmen_map_t map;
int rotation = 0;
double remain;
int degrees_angle;
carmen_offlimits_list_t offlimits_list;
carmen_map_placelist_t places_list;
next_arg = handle_options(argc, argv, &force);
next_arg++;
if(argc - next_arg != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s rotate <rotation in degrees> <in map filename> "
"<out map filename>\n\n", argv[0]);
}
degrees_angle = (int)(atof(argv[next_arg]) / 90);
remain = fabs(degrees_angle*90 - atof(argv[next_arg]));
if (carmen_radians_to_degrees(remain) > 2)
carmen_die("Rotations only supported in increments of 90 degrees.\n");
else
rotation = (int)atof(argv[next_arg]) / 90;
input_filename = check_mapfile(argv[next_arg+1]);
output_filename = check_output(argv[next_arg+2], force);
carmen_warn("Rotating by %d degrees\n", rotation*90);
/*
* Read the gridmap, places and offlimits chunks and rotate them
*/
ret_val = carmen_map_chunk_exists(input_filename, CARMEN_MAP_GRIDMAP_CHUNK);
if (ret_val < 0)
carmen_die_syserror("Couldn't check existence of GRIDMAP_CHUNK in %s",
input_filename);
if (carmen_map_read_gridmap_chunk(input_filename, &map) < 0)
carmen_die_syserror("Couldn't read GRIDMAP_CHUNK from %s", input_filename);
carmen_rotate_gridmap(&map, rotation);
ret_val = carmen_map_chunk_exists(input_filename,
CARMEN_MAP_OFFLIMITS_CHUNK);
if (ret_val > 0) {
ret_val = carmen_map_read_offlimits_chunk
(input_filename, &(offlimits_list.offlimits),
&(offlimits_list.list_length));
if (ret_val < 0)
carmen_die_syserror("Couldn't read OFFLIMITS_CHUNK in %s",
input_filename);
carmen_rotate_offlimits(map.config, &offlimits_list, rotation);
} else
offlimits_list.list_length = 0;
ret_val = carmen_map_chunk_exists(input_filename, CARMEN_MAP_PLACES_CHUNK);
if (ret_val > 0) {
ret_val = carmen_map_read_places_chunk(input_filename, &places_list);
if (ret_val < 0)
carmen_die_syserror("Couldn't read PLACES_CHUNK in %s", input_filename);
carmen_rotate_places(map.config, &places_list, rotation);
} else
places_list.num_places = 0;
/*
* Pass everything else through untouched, and then write the rotated
* chunks at the end.
*/
in_fp = carmen_fopen(input_filename, "r");
if (in_fp == NULL)
carmen_die_syserror("Couldn't open %s for reading", input_filename);
out_fp = carmen_fopen(output_filename, "w");
if (out_fp == NULL)
carmen_die_syserror("Couldn't open %s for writing", output_filename);
if (carmen_map_vstrip(in_fp, out_fp, 3, CARMEN_MAP_GRIDMAP_CHUNK,
CARMEN_MAP_OFFLIMITS_CHUNK,
CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die_syserror("Couldn't strip map to %s", output_filename);
if (carmen_map_write_gridmap_chunk(out_fp, map.map, map.config.x_size,
map.config.y_size,
map.config.resolution) < 0)
carmen_die_syserror("Couldn't write gridmap to %s", output_filename);
if (offlimits_list.list_length > 0) {
if (carmen_map_write_offlimits_chunk(out_fp, offlimits_list.offlimits,
offlimits_list.list_length) < 0)
carmen_die_syserror("Couldn't write offlimits list to %s",
output_filename);
}
if (places_list.num_places > 0) {
if (carmen_map_write_places_chunk(out_fp, places_list.places,
places_list.num_places) < 0)
carmen_die_syserror("Couldn't write places list to %s", output_filename);
}
carmen_fclose(in_fp);
carmen_fclose(out_fp);
}
void write_glv_output(logdata_p logdata, char *out_filename) {
int i, j, first_scan = 1;
transform_t t;
transform_point_t p;
double angle;
double d1, d2, d3, d4;
carmen_FILE *out_fp;
out_fp = carmen_fopen(out_filename, "w");
/* write path to glv file */
write_color_glv(out_fp, 255, 255, 0);
for (i = 1; i < logdata->num_pos; i++)
write_line_glv(out_fp,
logdata->pos[i-1].x, logdata->pos[i-1].y, logdata->pos[i-1].z,
logdata->pos[i].x, logdata->pos[i].y, logdata->pos[i].z);
/* write scan to glv file */
for (i = 0; i < logdata->num_laser; i++) {
if (!logdata->laser[i].ignore) {
int num_readings = logdata->laser[i].num_readings;
float local_x[num_readings];
point3D_t last_scan[num_readings], current_scan[num_readings];
char pointclass[num_readings], last_pointclass[num_readings];
if(i % 100 == 0)
fprintf(stderr, "\rProjecting points... (%.0f%%) ",
i/(float)logdata->num_laser*100.0);
transform_init_identity(t);
transform_rotate(t, logdata->laser[i].yaw, logdata->laser[i].pitch,
logdata->laser[i].roll);
transform_translate(t, logdata->laser[i].x, logdata->laser[i].y,
logdata->laser[i].z);
for (j = 0; j < num_readings; j++) {
angle = logdata->laser[i].start_angle+
j/(float)num_readings*logdata->laser[i].fov;
transform_point_init(&p,
logdata->laser[i].range[j]*cos(angle),
logdata->laser[i].range[j]*sin(angle),
0.0);
transform_point(t, &p);
pointclass[j] = TYPE_UNKNOWN;
if (classify_points) {
if (logdata->laser[i].range[j] > max_range)
pointclass[j] = TYPE_IGNORE;
else if (p.z > 0.0)
pointclass[j] = TYPE_OBSTACLE;
if (pointclass[j] == TYPE_UNKNOWN)
local_x[j] = hypot(p.x-logdata->laser[i].x, p.y-logdata->laser[i].y);
}
last_scan[j] = current_scan[j];
last_pointclass[j] = pointclass[j];
current_scan[j].x = p.x;
current_scan[j].y = p.y;
current_scan[j].z = p.z;
current_scan[j].meshed = 0;
current_scan[j].range = logdata->laser[i].range[j];
}
if (classify_points)
classify_scan(num_readings, current_scan, local_x, pointclass);
write_color_glv(out_fp, 255, 255, 255);
for (j = 0; j < num_readings; j++) {
if (generate_faces && !first_scan && j > 0) {
d1 = logdata->laser[i].range[j-1]-logdata->laser[i-1].range[j-1];
d2 = logdata->laser[i].range[j]-logdata->laser[i].range[j-1];
d3 = logdata->laser[i-1].range[j]-logdata->laser[i].range[j];
d4 = logdata->laser[i-1].range[j-1]-logdata->laser[i-1].range[j];
if (fabs(d1) < connect_dist && fabs(d2) < connect_dist &&
fabs(d3) < connect_dist && fabs(d4) < connect_dist &&
logdata->laser[i].range[j-1] < max_range &&
logdata->laser[i-1].range[j-1] < max_range &&
logdata->laser[i].range[j] < max_range &&
logdata->laser[i-1].range[j] < max_range) {
if (pointclass[j] == TYPE_OBSTACLE ||
pointclass[j-1] == TYPE_OBSTACLE)
write_color_glv(out_fp, 100, 100, 255);
else
write_color_glv(out_fp, 255, 255, 255);
write_face_glv(out_fp,
last_scan[j-1].x, last_scan[j-1].y, last_scan[j-1].z,
current_scan[j-1].x, current_scan[j-1].y, current_scan[j-1].z,
current_scan[j].x, current_scan[j].y, current_scan[j].z);
write_face_glv(out_fp,
last_scan[j-1].x, last_scan[j-1].y, last_scan[j-1].z,
current_scan[j].x, current_scan[j].y, current_scan[j].z,
last_scan[j].x, last_scan[j].y, last_scan[j].z);
last_scan[j].meshed = 1;
last_scan[j-1].meshed = 1;
current_scan[j].meshed = 1;
current_scan[j-1].meshed = 1;
}
}
}
if (generate_faces) {
write_color_glv(out_fp, 255, 255, 255);
for (j = 0; j < num_readings; j++) {
if (last_pointclass[j] == TYPE_SAFE &&
!last_scan[j].meshed && !first_scan &&
last_scan[j].range < max_range)
write_point_glv(out_fp, last_scan[j].x, last_scan[j].y,
last_scan[j].z);
}
write_color_glv(out_fp, 100, 100, 255);
for (j = 0; j < num_readings; j++) {
if (last_pointclass[j] == TYPE_OBSTACLE &&
!last_scan[j].meshed && !first_scan &&
last_scan[j].range < max_range)
write_point_glv(out_fp, last_scan[j].x, last_scan[j].y,
last_scan[j].z);
}
}
else {
write_color_glv(out_fp, 255, 255, 255);
for (j = 0; j < num_readings; j++) {
if ((pointclass[j] == TYPE_SAFE) || (pointclass[j] == TYPE_UNKNOWN))
write_point_glv(out_fp, current_scan[j].x, current_scan[j].y,
current_scan[j].z);
}
write_color_glv(out_fp, 100, 100, 255);
for (j = 0; j < num_readings; j++) {
if (pointclass[j] == TYPE_OBSTACLE)
write_point_glv(out_fp, current_scan[j].x, current_scan[j].y,
current_scan[j].z);
}
}
first_scan = 0;
}
}
fprintf(stderr, "\rProjecting points... (100%%) \n");
carmen_fclose(out_fp);
}
int
main(int argc, char **argv)
{
int width = 0, height = 0;
double resolution = 0.0;
char *filename = NULL;
carmen_map_t map;
int index;
carmen_FILE *out_file;
carmen_read_commandline_parameters(argc, argv);
if (argc > 1)
{
if (strcmp(argv[argc-1], "-w") != 0 &&
strcmp(argv[argc-1], "-h") != 0 &&
strcmp(argv[argc-2], "-w") != 0 &&
strcmp(argv[argc-2], "-h") != 0)
filename = argv[argc-1];
}
carmen_process_param_int("w", usage, &width);
carmen_process_param_int("h", usage, &height);
carmen_process_param_double("r", usage, &resolution);
if (width <= 0)
{
printf("Map width: ");
scanf("%d", &width);
}
if (height <= 0)
{
printf("Map height: ");
scanf("%d", &height);
}
if (resolution <= 0)
{
printf("Map resolution: ");
scanf("%lf", &resolution);
}
if (filename == NULL)
{
printf("Filename: ");
filename = (char *)calloc(1024, sizeof(char));
carmen_test_alloc(filename);
scanf("%s", filename);
}
printf("Creating map %d x %d, resolution %.2f, storing in %s\n",
width, height, resolution, filename);
map.config.x_size = width;
map.config.y_size = height;
map.config.resolution = resolution;
map.config.map_name = filename;
map.complete_map = (float *)calloc(width*height, sizeof(float));
carmen_test_alloc(map.complete_map);
for (index = 0; index < width*height; index++)
map.complete_map[index] = -1;
map.map = (float **)calloc(width, sizeof(float *));
carmen_test_alloc(map.map);
for (index = 0; index < width; index++)
map.map[index] = map.complete_map+index*height;
out_file = carmen_fopen(filename, "w");
if (out_file == NULL)
carmen_die("Couldn't open %s for writing map: %s\n", filename,
strerror(errno));
carmen_map_write_id(out_file);
carmen_map_write_creator_chunk(out_file, "generate_blank", "blank map");
carmen_map_write_gridmap_chunk(out_file, map.map, width, height, resolution);
carmen_fclose(out_file);
return 0;
}
