int
carmen_parse_arm_joint_types(char *joint_string, carmen_arm_joint_t *joint_types,
int num_joints)
{
char *s;
int n, i;
if (!joint_string || !joint_types)
{
carmen_warn("Bug in carmen_parse_arm_joint_types: a pointer arg was "
"passed as NULL\n");
return -1;
}
s = joint_string;
for (i = 0; i < num_joints; i++) {
s += strspn(s, " \t");
if (strlen(s) == 0) {
carmen_warn("Too few arguments in joint types string.\n");
return -1;
}
n = strcspn(s, " \t");
if (!carmen_strncasecmp(s, "motor", n))
joint_types[i] = CARMEN_MOTOR;
else if (!carmen_strncasecmp(s, "servo", n))
joint_types[i] = CARMEN_SERVO;
else {
carmen_warn("Bad argument (#%d) in joint types string.\n", i+1);
return -1;
}
}
return 0;
}
static int
find_valid_robots(char **robot_names, int *num_robots, int max_robots)
{
FILE *fp = NULL;
char *err, *line, *mark;
char *left, *right;
int count = 0;
fp = fopen(param_filename, "r");
if(fp == NULL)
return -1;
line = (char *)calloc(MAX_VARIABLE_LENGTH, sizeof(char));
carmen_test_alloc(line);
do {
err = fgets(line, MAX_VARIABLE_LENGTH-1, fp);
line[MAX_VARIABLE_LENGTH-1] = '\0';
if(strlen(line) == MAX_VARIABLE_LENGTH-1)
carmen_die("Line %d of file %s is too long.\n"
"Maximum line length is %d. Please correct this line.\n\n"
"It is also possible that this file has become corrupted.\n"
"Make sure you have an up-to-date version of carmen, and\n"
"consult the param_server documentation to make sure the\n"
"file format is valid.\n", count, param_filename,
MAX_VARIABLE_LENGTH-1);
count++;
if(err != NULL) {
mark = strchr(line, '#'); /* strip comments and trailing returns */
if(mark != NULL)
mark[0] = '\0';
mark = strchr(line, '\n');
if(mark != NULL)
mark[0] = '\0';
left = strchr(line, '[');
right = strchr(line, ']');
if(left != NULL && right != NULL && left < right &&
left[1] != '*' && carmen_strncasecmp(left+1, "expert", 6) != 0) {
(*num_robots)++;
if(*num_robots > max_robots)
carmen_die("Error: exceeded maximum number of robots in parameter file (%d).\n", max_robots);
robot_names[*num_robots - 1] =
(char *)calloc(right - left, 1);
carmen_test_alloc(robot_names[*num_robots - 1]);
strncpy(robot_names[*num_robots - 1], left + 1, right - left - 1);
robot_names[*num_robots - 1][right - left - 1] = '\0';
}
}
} while(err != NULL);
free(line);
return 0;
}
static int
read_parameters_from_file(void)
{
FILE *fp = NULL;
char *line;
char *mark, *token;
int token_num;
char lvalue[255], rvalue[MAX_VARIABLE_LENGTH];
int found_matching_robot = 0;
int found_desired_robot = 0;
int expert = 0;
int line_length;
int count;
fp = fopen(param_filename, "r");
if(fp == NULL)
return -1;
line = (char *)calloc(MAX_VARIABLE_LENGTH, sizeof(char));
carmen_test_alloc(line);
count = 0;
while (!feof(fp)) {
fgets(line, MAX_VARIABLE_LENGTH-1, fp);
line[MAX_VARIABLE_LENGTH-1] = '\0';
if (strlen(line) == MAX_VARIABLE_LENGTH-1)
carmen_die("Line %d of file %s is too long.\n"
"Maximum line length is %d. Please correct this line.\n\n"
"It is also possible that this file has become corrupted.\n"
"Make sure you have an up-to-date version of carmen, and\n"
"consult the param_server documentation to make sure the\n"
"file format is valid.\n", count, param_filename,
MAX_VARIABLE_LENGTH-1);
count++;
if (feof(fp))
break;
mark = strchr(line, '#'); /* strip comments and trailing returns */
if (mark != NULL)
mark[0] = '\0';
mark = strchr(line, '\n');
if (mark != NULL)
mark[0] = '\0';
// Trim off trailing white space
line_length = strlen(line) - 1;
while (line_length >= 0 &&
(line[line_length] == ' ' || line[line_length] == '\t' )) {
line[line_length--] = '\0';
}
line_length++;
if (line_length == 0)
continue;
// Skip over initial blank space
mark = line + strspn(line, " \t");
if (strlen(mark) == 0)
carmen_die("You have encountered a bug in carmen. Please report it\n"
"to the carmen maintainers. \n"
"Line %d, function %s, file %s\n", __LINE__, __FUNCTION__,
__FILE__);
token_num = 0;
/* tokenize line */
token = mark;
// Move mark to the first whitespace character.
mark = strpbrk(mark, " \t");
// If we found a whitespace character, then turn it into a NULL
// and move mark to the next non-whitespace.
if (mark) {
mark[0] = '\0';
mark++;
mark += strspn(mark, " \t");
}
if (strlen(token) > 254) {
carmen_warn("Bad file format of %s on line %d.\n"
"The parameter name %s is too long (%d characters).\n"
"A parameter name can be no longer than 254 "
"characters.\nSkipping this line.\n", param_filename,
count, token, (int) strlen(token));
continue;
}
strcpy(lvalue, token);
token_num++;
// If mark points to a non-whitespace character, then we have a
// two-token line
if (mark) {
if (strlen(mark) > MAX_VARIABLE_LENGTH-1) {
carmen_warn("Bad file format of %s on line %d.\n"
"The parameter value %s is too long (%d "
"characters).\nA parameter value can be no longer "
"than %u characters.\nSkipping this line.\n",
param_filename, count, mark, (int) strlen(mark),
MAX_VARIABLE_LENGTH-1);
continue;
}
strcpy(rvalue, mark);
token_num++;
}
if (lvalue[0] == '[') {
if (strcspn(lvalue+1,"]") == 6 && !carmen_strncasecmp(lvalue+1, "expert", 6)) {
found_matching_robot = 1;
expert = 1;
}
else {
expert = 0;
if (lvalue[1] == '*')
found_matching_robot = 1;
else if (selected_robot) {
if (strlen(lvalue) < strlen(selected_robot) + 2)
found_matching_robot = 0;
else if (lvalue[strlen(selected_robot)+1] != ']')
found_matching_robot = 0;
else if (carmen_strncasecmp
(lvalue+1, selected_robot, strlen(selected_robot)) == 0) {
found_matching_robot = 1;
found_desired_robot = 1;
} else
found_matching_robot = 0;
}
}
}
else if(token_num == 2 && found_matching_robot == 1)
set_param(lvalue, rvalue, expert);
} /* End of while (!feof(fp)) */
fclose(fp);
if (selected_robot && !found_desired_robot) {
carmen_die("Did not find a match for robot %s. Do you have the right\n"
"init file? (Reading parameters from %s)\n", selected_robot,
param_filename);
}
return 0;
}
int main(int argc, char *argv[])
{
struct stat buf;
double x, y, theta, tv, rv;
double left_disp, right_disp, delta_time;
int fd;
int unused_return_value;
if (argc != 2 || carmen_strncasecmp(argv[1], "-h", 2) == 0 ||
carmen_strncasecmp(argv[1], "--h", 3) == 0) {
if (argc > 2)
carmen_warn("Too many arguments.\n\n");
else if (argc < 2)
carmen_warn("Not enough arguments.\n\n");
carmen_die("Usage: %s <serial dev name>\n", argv[0]);
}
// Does the device exist?
carmen_warn("Device exists test: ");
if (stat(argv[1], &buf) == 0)
carmen_warn("OK\n");
else {
carmen_warn("FAILED\n");
carmen_die_syserror("Device exists test");
}
// Can we open the device?
carmen_warn("Device open test: ");
fd = open(argv[1], O_RDWR | O_SYNC | O_NOCTTY, 0);
if (fd >= 0)
carmen_warn("OK\n");
else {
carmen_warn("FAILED\n");
carmen_die_syserror("Device open test");
}
// close(fd);
// Can we talk to the orc board properly?
carmen_warn("Orc recognized test: ");
if (carmen_base_direct_initialize_robot("orc", argv[1]) == 0)
carmen_warn("OK\n");
else {
carmen_die("FAILED\n");
}
// Can we move the wheels?
carmen_warn("Orc drive test: %sMAKE SURE THE ROBOT IS ON BLOCKS%s.\n",
carmen_red_code, carmen_normal_code);
carmen_warn("Hit return to start the orc drive test....");
unused_return_value = scanf("%*c");
if (1) {
// Move left wheel
carmen_warn("Left wheel forwards test: ");
carmen_base_command_velocity(1, 0, 0);
sleep(1);
carmen_base_command_velocity(0, 0, 0);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(right_disp) > 1e-3)
carmen_die("FAILED\nRight encoder moved. The motors need "
"to be swapped.\n");
if (left_disp < -1e-3)
carmen_die("FAILED\n Left encoder moved backwards. The motor is "
"upside-down.\n");
if (fabs(left_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
carmen_warn("Left wheel backwards test: ");
carmen_base_command_velocity(-1, 0, 0);
sleep(1);
carmen_base_command_velocity(0, 0, 0);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(right_disp) > 1e-3)
carmen_die("FAILED\nRight encoder moved. Very odd.\n");
if (left_disp > 1e-3)
carmen_die("FAILED\n Left encoder moved forwards. Very odd.\n");
if (fabs(left_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
// Move right wheel
carmen_warn("Right wheel forwards test: ");
carmen_base_command_velocity(1, 0, 2);
sleep(1);
carmen_base_command_velocity(0, 0, 2);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(left_disp) > 1e-3)
carmen_die("FAILED\nLeft encoder moved. This is very odd, since it"
"also moved with\nthe other wheel.\n");
if (right_disp < -1e-3)
carmen_die("FAILED\nRight encoder moved backwards. The motor is "
"upside-down.\n");
if (fabs(right_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
carmen_warn("OK\n");
carmen_warn("Right wheel backwards test: ");
carmen_base_command_velocity(-1, 0, 2);
sleep(1);
carmen_base_command_velocity(0, 0, 2);
sleep(1);
carmen_base_query_low_level(&left_disp, &right_disp, &delta_time);
// carmen_warn("%f %f %f\n", left_disp, right_disp, delta_time);
if (0 && fabs(left_disp) > 1e-3)
carmen_die("FAILED\nLeft encoder moved. Very odd.\n");
if (right_disp > 1e-3)
carmen_die("FAILED\nRight encoder moved forwards. Very odd.\n");
if (fabs(right_disp) < 1e-3)
carmen_die("FAILED\nEncoder failed to move. Did the wheel move?\n");
}
carmen_warn("OK\n");
carmen_base_direct_reset();
carmen_warn("Orc drive test: ");
if (carmen_base_direct_set_velocity(1.0, 0.0) == 0)
carmen_warn("OK\n");
else
carmen_die("FAILED\n");
// Let some encoder data build up
carmen_warn("Sleep\n");
sleep(1);
carmen_base_direct_set_velocity(0.0, 0.0);
// Are we getting encoder data?
carmen_warn("Orc encoder test: ");
if (carmen_base_direct_get_integrated_state(&x, &y, &theta, &tv, &rv) == 0)
carmen_warn("OK\n");
else
carmen_die("FAILED\n");
// Is encoder data valid?
carmen_warn("Orc encoder data validity test: ");
if (fabs(x) > 0 && fabs(y) < 1e-2 &&
fabs(carmen_radians_to_degrees(theta)) < 10)
carmen_warn("OK (%.2f)\n", x);
else
carmen_die("FAILED (%.2f %.2f %.2f)\n", x, y,
carmen_radians_to_degrees(theta));
carmen_base_direct_shutdown_robot();
return 0;
}
