int iKXPlugin::save_preset(const char *new_name)
{
kSettings cfg;
char full_key[127+KX_MAX_STRING];
sprintf(full_key,"Plugins\\%s",get_plugin_description(IKX_PLUGIN_GUID));
kxparam_t *data=0;
int cnt=get_param_count();
data=(kxparam_t *)malloc(cnt*sizeof(kxparam_t));
if(data)
{
get_all_params(data);
cfg.write_bin_abs(full_key,new_name,data,cnt*sizeof(kxparam_t));
free(data);
}
else return -1;
return 0;
}
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 iKXPlugin::get_current_preset(char *ret_name)
{
int found=0;
kxparam_t *data=0;
int cnt=get_param_count();
if(cnt)
data=(kxparam_t *)malloc(cnt*sizeof(kxparam_t));
if(data)
{
get_all_params(data);
kSettings cfg;
char full_key[127+KX_MAX_STRING];
sprintf(full_key,"Plugins\\%s",get_plugin_description(IKX_PLUGIN_GUID));
int ndx=0;
while(1)
{
char key_name[KX_MAX_STRING+256];
kxparam_t value[256];
if(cfg.enum_abs(ndx,full_key,key_name,sizeof(key_name),(TCHAR *)value,sizeof(value))==0)
{
if(memcmp(value,data,cnt*sizeof(kxparam_t))==0)
{
strncpy(ret_name,key_name,KX_MAX_STRING);
found=1;
goto OK;
}
}
else
{
break;
}
ndx++;
}
// second, check built-in presets
try
{
const kxparam_t *t=get_plugin_presets();
if(t)
{
int num=get_param_count();
while(1)
{
char *name=(char *)t[0];
if(name==0)
break;
if(memcmp(data,&t[1],num*sizeof(kxparam_t))==0)
{
strncpy(ret_name,name,KX_MAX_STRING);
found=1;
goto OK;
}
t+=(num+1);
}
}
}
catch(...)
{
debug("Incorrect plugin: %d [pointer from get_current_preset()]\n",__LINE__);
}
OK:
free(data);
}
return found?0:-1;
}
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;
}
