char *SETUP_ExtGetValue(char *Section, char *Key)
{
char *tmp, *ret;
tmp = calloc(strlen(Section) + strlen(Key) + 3, 1);
carmen_test_alloc(tmp);
sprintf(tmp, "[%s]%s", Section, Key);
ret = SETUP_GetValue(tmp);
free(tmp);
return ret;
}
long int SETUP_ItemCount(char *StemKey)
{
char *ItemKey;
long int ItemCount;
ItemCount = 0;
ItemKey = (char *)calloc(strlen(StemKey) + 0x226, 1);
carmen_test_alloc(ItemKey);
sprintf(ItemKey, "%s%ld", StemKey, ItemCount);
while (SETUP_GetValue(ItemKey) != NULL) {
ItemCount++;
sprintf(ItemKey, "%s%ld", StemKey, ItemCount);
}
return ItemCount;
}
void BUMP_Initialize(struct N_RobotState *rs, struct N_RobotStateExt *rs_ext)
{
char *const_string;
char **set_argv;
char **bump_argv;
unsigned short s;
unsigned short bumper_index;
int set_argc;
int bump_argc;
int argc_index;
struct N_XBumperSet *xset;
const_string = SETUP_GetValue("[bumper]bumper_sets");
assert(const_string != ((void *)0));
ARGCV_Create(const_string, &set_argc, &set_argv);
if (set_argc > N_MAX_BUMPER_SET_COUNT) {
fprintf(stderr, "Setup: too many bumper sets in %s.\n"
" Using the first %d.\n", "[bumper]bumper_sets",
N_MAX_BUMPER_SET_COUNT);
rs->BumperController.BumperSetCount = N_MAX_BUMPER_SET_COUNT;
} else {
rs->BumperController.BumperSetCount = set_argc;
}
for (s = 0; s < rs->BumperController.BumperSetCount; s++) {
xset = &(rs_ext->BumperController.BumperSet[s]);
if ((const_string = SETUP_ExtGetValue(set_argv[s], "bumpers")) == NULL) {
rs->BumperController.BumperSet[s].BumperCount = 0;
} else {
ARGCV_Create(const_string, &bump_argc, &bump_argv);
argc_index = 0;
bumper_index = 0;
while (bumper_index < N_MAX_BUMPER_COUNT) {
if (argc_index == bump_argc)
break;
const_string = SETUP_ExtGetValue(set_argv[s], bump_argv[argc_index]);
if (const_string == NULL) {
fprintf(stderr, "Warning: could not find a setup entry for [%s]%s\n",
set_argv[s], bump_argv[argc_index]);
argc_index++;
continue;
}
if (ParseBumperCommand(xset, &bumper_index, set_argv[s],
const_string) < 0) {
fprintf(stderr, "Bumper (warning): invalid bumper "
"specification after [%s]%s.\n",
set_argv[s], bump_argv[argc_index]);
}
argc_index++;
}
rs->BumperController.BumperSet[s].BumperCount = bumper_index;
ARGCV_Free(bump_argc, bump_argv);
}
}
ARGCV_Free(set_argc, set_argv);
return ;
}
void INF_Initialize(struct N_RobotState *rs, struct N_RobotStateExt *rs_ext)
{
unsigned short s;
unsigned short ir;
char *const_string;
char **set_argv;
char **ir_argv;
int set_argc;
int ir_argc;
struct N_XInfraredSet *xset;
const_string = SETUP_GetValue("[infrared]infrared_sets");
assert(const_string != ((void *)0));
ARGCV_Create(const_string, &set_argc, &set_argv);
if (set_argc > N_MAX_INFRARED_SET_COUNT) {
printf("Warning: too many infrared sets in %s.\n"
" Using the first %d.\n", "[infrared]infrared_sets",
N_MAX_INFRARED_SET_COUNT);
rs->InfraredController.InfraredSetCount = N_MAX_INFRARED_SET_COUNT;
} else {
rs->InfraredController.InfraredSetCount = set_argc;
}
for (s = 0; s < rs->InfraredController.InfraredSetCount; s++) {
xset = &(rs_ext->InfraredController.InfraredSet[s]);
const_string = SETUP_ExtGetValue(set_argv[s], "main_lobe");
assert(const_string != ((void *)0));
xset->MainLobe = atof(const_string);
const_string = SETUP_ExtGetValue(set_argv[s], "range");
assert(const_string != ((void *)0));
xset->Range = atof(const_string);
const_string = SETUP_ExtGetValue(set_argv[s], "infrareds");
assert(const_string != ((void *)0));
ARGCV_Create(const_string, &ir_argc, &ir_argv);
if (ir_argc > N_MAX_INFRARED_COUNT) {
printf("Warning: too many infrareds in [%s]%s.\n"
" Using the first %d.\n", set_argv[s], "infrareds",
N_MAX_INFRARED_COUNT);
rs->InfraredController.InfraredSet[s].InfraredCount =
N_MAX_INFRARED_COUNT;
} else {
rs->InfraredController.InfraredSet[s].InfraredCount = ir_argc;
}
for (ir = 0; ir < rs->InfraredController.InfraredSet[s].InfraredCount;
ir++) {
if ((const_string = SETUP_ExtGetValue(set_argv[s], ir_argv[ir])) ==
NULL) {
fprintf(stderr, "Warning: could not find a setup entry for [%s]%s\n",
set_argv[s], ir_argv[ir]);
} else {
ParseInfraredCommand(&(xset->Infrared[ir]), const_string,
set_argv[s]);
}
}
ARGCV_Free(ir_argc, ir_argv);
}
ARGCV_Free(set_argc, set_argv);
return ;
}
void SON_Initialize(struct N_RobotState *rs, struct N_RobotStateExt *rs_ext)
{
unsigned short s;
unsigned short sn;
char *const_string;
char **set_argv;
char **son_argv;
int set_argc;
int son_argc;
struct N_SonarSet *sonar_set;
struct N_XSonarSet *xset;
const_string = SETUP_GetValue("[sonar]sonar_sets");
assert(const_string != ((void *)0));
ARGCV_Create(const_string, &set_argc, &set_argv);
if (set_argc > N_MAX_SONAR_SET_COUNT) {
fprintf(stderr, "Setup: too many sonar sets in %s.\n"
" Using the first %d.\n", "[sonar]sonar_sets",
N_MAX_SONAR_SET_COUNT);
rs->SonarController.SonarSetCount = N_MAX_SONAR_SET_COUNT;
} else {
rs->SonarController.SonarSetCount = set_argc;
}
for (s = 0; s < rs->SonarController.SonarSetCount; s++) {
sonar_set = &(rs->SonarController.SonarSet[s]);
xset = &(rs_ext->SonarController.SonarSet[s]);
const_string = SETUP_ExtGetValue(set_argv[s], "main_lobe");
xset->MainLobe = (const_string != NULL) ? atof(const_string) : 0.0;
const_string = SETUP_ExtGetValue(set_argv[s], "blind_lobe");
xset->BlindLobe = (const_string != NULL) ? atof(const_string) : 0.0;
const_string = SETUP_ExtGetValue(set_argv[s], "side_lobe");
xset->SideLobe = (const_string != NULL) ? atof(const_string) : 0.0;
const_string = SETUP_ExtGetValue(set_argv[s], "range");
xset->Range = (const_string != NULL) ? atof(const_string) : 0.0;
const_string = SETUP_ExtGetValue(set_argv[s], "blind_lobe_attenuation");
xset->BlindLobeAttenuation =
(const_string != NULL) ? atof(const_string) : 0.0;
const_string = SETUP_ExtGetValue(set_argv[s], "side_lobe_attenuation");
xset->SideLobeAttenuation =
(const_string != NULL) ? atof(const_string) : 0.0;
const_string = SETUP_ExtGetValue(set_argv[s], "sonars");
if (const_string == NULL) {
sonar_set->SonarCount = 0;
} else {
ARGCV_Create(const_string, &son_argc, &son_argv);
if (son_argc > N_MAX_SONAR_COUNT) {
printf("Warning: too many sonars in [%s]%s.\n"
" Using the first %d.\n", set_argv[s], "sonars",
N_MAX_SONAR_COUNT);
sonar_set->SonarCount = N_MAX_SONAR_COUNT;
} else {
sonar_set->SonarCount = son_argc;
}
for (sn = 0; sn < sonar_set->SonarCount; sn++) {
const_string = SETUP_ExtGetValue(set_argv[s], son_argv[sn]);
if (const_string == NULL) {
fprintf(stderr, "Warning: could not find a setup entry for [%s]%s\n",
set_argv[s], son_argv[sn]);
} else {
ParseSonarCommand(&(xset->Sonar[sn]), const_string, set_argv[s]);
}
}
ARGCV_Free(son_argc, son_argv);
}
}
ARGCV_Free(set_argc, set_argv);
return;
}