void
Robot::draw()
{
tf::Transform robot_pose, opengl_to_robot_pose;
robot_pose.setOrigin(tf::Vector3(getRobotPose().position.x, getRobotPose().position.y, getRobotPose().position.z));
robot_pose.setRotation(tf::Quaternion(getRobotPose().orientation.yaw, getRobotPose().orientation.pitch, getRobotPose().orientation.roll));
opengl_to_robot_pose.setOrigin(tf::Vector3(1.4, 0.0, 0.0));
opengl_to_robot_pose.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
robot_pose = robot_pose * opengl_to_robot_pose;
if(this->robot_model_ != NULL)
{
glPushMatrix();
glTranslatef(robot_pose.getOrigin().x(),robot_pose.getOrigin().y(), robot_pose.getOrigin().z() + 0.28);
double y, p, r;
tf::Matrix3x3(robot_pose.getRotation()).getRPY(r, p, y);
glRotatef(carmen_radians_to_degrees(y), 0.0f, 0.0f, 1.0f);
glRotatef(carmen_radians_to_degrees(p), 0.0f, 1.0f, 0.0f);
glRotatef(carmen_radians_to_degrees(r), 1.0f, 0.0f, 0.0f);
glRotatef(90.0, 1.0, 0.0, 0.0);
glRotatef(0.0, 0.0, 1.0, 0.0);
glmDraw(this->robot_model_, GLM_SMOOTH | GLM_COLOR);
glPopMatrix();
}
}
void
draw_orientation_instruments (carmen_orientation_3D_t orientation, double r, double g, double b)
{
glPushMatrix ();
glColor3d (r, g, b);
glLoadIdentity ();
glTranslated (-0.16, -0.16, -0.5);
glPushMatrix ();
glRotated (carmen_radians_to_degrees (orientation.yaw), 0.0, 0.0, 1.0);
draw_arrow (0.05);
glPopMatrix ();
glTranslated (0.1, 0.0, 0.0);
glPushMatrix ();
glRotated (carmen_radians_to_degrees (orientation.pitch), 0.0, 0.0, -1.0);
draw_arrow (0.05);
glPopMatrix ();
glTranslated (0.1, 0.0, 0.0);
glPushMatrix ();
glRotated (carmen_radians_to_degrees (orientation.roll), 0.0, 0.0, 1.0);
draw_arrow (0.05);
glPopMatrix ();
glPopMatrix ();
}
void draw_car_at_pose(CarDrawer* carDrawer, carmen_pose_3D_t pose)
{
glPushMatrix();
glTranslatef(pose.position.x, pose.position.y, pose.position.z);
glRotatef(carmen_radians_to_degrees(pose.orientation.yaw), 0.0f, 0.0f, 1.0f);
glRotatef(carmen_radians_to_degrees(pose.orientation.pitch), 0.0f, 1.0f, 0.0f);
glRotatef(carmen_radians_to_degrees(pose.orientation.roll), 1.0f, 0.0f, 0.0f);
draw_car(carDrawer);
glPopMatrix();
}
void robot_rearlaser_handler(carmen_robot_laser_message *rear_laser)
{
carmen_warn("\nrear_laser\n");
carmen_warn("%.2f %.2f %.2f\n", rear_laser->robot_pose.x,
rear_laser->robot_pose.y,
carmen_radians_to_degrees(rear_laser->robot_pose.theta));
}
void
draw_xsens_orientation (carmen_orientation_3D_t xsens_orientation, double xsens_yaw_bias, carmen_pose_3D_t xsens_pose, carmen_pose_3D_t sensor_board_pose, carmen_pose_3D_t car_pose)
{
carmen_vector_3D_t xsens_global_position = get_xsens_position_global_reference (xsens_pose, sensor_board_pose, car_pose);
glPushMatrix ();
glTranslatef (xsens_global_position.x, xsens_global_position.y, xsens_global_position.z);
glRotatef (carmen_radians_to_degrees (xsens_orientation.yaw - xsens_yaw_bias), 0.0f, 0.0f, 1.0f);
glRotatef (carmen_radians_to_degrees (xsens_orientation.pitch), 0.0f, 1.0f, 0.0f);
glRotatef (carmen_radians_to_degrees (xsens_orientation.roll), 1.0f, 0.0f, 0.0f);
draw_axis (1.0);
glPopMatrix ();
}
void robot_frontlaser_handler(carmen_robot_laser_message *front_laser)
{
carmen_warn("front_laser\n");
carmen_warn("%.2f %.2f %.2f\n", front_laser->robot_pose.x,
front_laser->robot_pose.y,
carmen_radians_to_degrees(front_laser->robot_pose.theta));
}
void
draw_gps_orientation (double gps_orientation, int gps_heading_valid, carmen_orientation_3D_t xsens_orientation, carmen_pose_3D_t xsens_pose, carmen_pose_3D_t sensor_board_pose, carmen_pose_3D_t car_pose)
{
carmen_vector_3D_t xsens_global_position = get_xsens_position_global_reference (xsens_pose, sensor_board_pose, car_pose);
glPushMatrix ();
glTranslatef (xsens_global_position.x, xsens_global_position.y, xsens_global_position.z);
glRotatef (carmen_radians_to_degrees (gps_orientation), 0.0f, 0.0f, 1.0f);
glRotatef (carmen_radians_to_degrees (xsens_orientation.pitch), 0.0f, 1.0f, 0.0f);
glRotatef (carmen_radians_to_degrees (xsens_orientation.roll), 1.0f, 0.0f, 0.0f);
if (gps_heading_valid)
draw_axis (1.0);
else
draw_axis (0.5);
glPopMatrix ();
}
double *
get_variable_velodyne_correction()
{
double *vert_angle = (double*) calloc(64,sizeof(double));
carmen_test_alloc(vert_angle);
// angulo vertical minimo
double vmin = carmen_radians_to_degrees(-0.45);
// angulo vertical maximo
double vmax = carmen_radians_to_degrees(0.111);
// intervalo angular vertical
double vstep = (vmax - vmin) / 64.0;
for(int i = 0; i < 64; i++) {
vert_angle[i] = vmin + vstep * i;
}
return vert_angle;
}
static void
fill_laser_config_data(localize_ackerman_velodyne_laser_config_t *lasercfg)
{
lasercfg->num_lasers = 1 + carmen_round(lasercfg->fov / lasercfg->angular_resolution);
lasercfg->start_angle = -0.5 * lasercfg->fov;
/* give a warning if it is not a standard configuration */
if (fabs(lasercfg->fov - M_PI) > 1e-6 &&
fabs(lasercfg->fov - 100.0/180.0 * M_PI) > 1e-6 &&
fabs(lasercfg->fov - 90.0/180.0 * M_PI) > 1e-6)
carmen_warn("Warning: You are not using a standard SICK configuration (fov=%.4f deg)\n",
carmen_radians_to_degrees(lasercfg->fov));
if (fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(1.0)) > 1e-6 &&
fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(0.5)) > 1e-6 &&
fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(0.25)) > 1e-6)
carmen_warn("Warning: You are not using a standard SICK configuration (res=%.4f deg)\n",
carmen_radians_to_degrees(lasercfg->angular_resolution));
}
void draw_velodyne_360(velodyne_360_drawer* v_drawer, carmen_pose_3D_t car_pose)
{
glPushMatrix();
glTranslatef(car_pose.position.x, car_pose.position.y, car_pose.position.z);
glRotatef(carmen_radians_to_degrees(car_pose.orientation.yaw), 0.0f, 0.0f, 1.0f);
glRotatef(carmen_radians_to_degrees(car_pose.orientation.pitch), 0.0f, 1.0f, 0.0f);
glRotatef(carmen_radians_to_degrees(car_pose.orientation.roll), 1.0f, 0.0f, 0.0f);
glBegin(GL_POINTS);
//glColor3f(0.0,0.0,1.0);
int num_points_current = v_drawer->num_points_current;
int i;
for(i=0; i<num_points_current; i++)
{
set_laser_point_color(v_drawer->points_current[i].x, v_drawer->points_current[i].y, v_drawer->points_current[i].z);
glVertex3f(v_drawer->points_current[i].x, v_drawer->points_current[i].y, v_drawer->points_current[i].z);
}
int num_points_last = v_drawer->num_points_last;
if(num_points_current > 0)
{
double last_current_angle = v_drawer->angles_current[num_points_current - 1];
int last_start_index = get_index_angle_last(v_drawer, last_current_angle);
for(i=last_start_index; i>=0 && i<num_points_last; i++)
{
set_laser_point_color(v_drawer->points_last[i].x, v_drawer->points_last[i].y, v_drawer->points_last[i].z);
glVertex3f(v_drawer->points_last[i].x, v_drawer->points_last[i].y, v_drawer->points_last[i].z);
}
}
glEnd();
glPopMatrix();
}
void navigator_update_robot(carmen_world_point_p robot)
{
carmen_point_t std = {0.2, 0.2, carmen_degrees_to_radians(4.0)};
if (robot == NULL) {
carmen_localize_initialize_uniform_command();
} else {
carmen_verbose("Set robot position to %d %d %f\n",
carmen_round(robot->pose.x),
carmen_round(robot->pose.y),
carmen_radians_to_degrees(robot->pose.theta));
carmen_localize_initialize_gaussian_command(robot->pose, std);
}
}
void
reset_camera ()
{
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glClearColor (background_r, background_g, background_b, 1.0f);
glClearDepth (1.0);
glDepthFunc (GL_LESS);
glEnable (GL_DEPTH_TEST);
glShadeModel (GL_SMOOTH);
glEnable (GL_LIGHT1);
glEnable (GL_LIGHTING);
glColorMaterial (GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glEnable (GL_COLOR_MATERIAL);
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
gluPerspective (45.0f, WINDOW_WIDTH / WINDOW_HEIGHT, 0.1f, 4000.0f);
;
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
glLoadIdentity ();
glRotated (90.0, 0.0, 0.0, 1.0);
glRotated (90.0, 0.0, 1.0, 0.0);
glRotated (carmen_radians_to_degrees (camera_pose.orientation.roll), -1.0, 0.0, 0.0);
glRotated (carmen_radians_to_degrees (camera_pose.orientation.pitch), 0.0, -1.0, 0.0);
glRotated (carmen_radians_to_degrees (camera_pose.orientation.yaw), 0.0, 0.0, -1.0);
glTranslated (-camera_pose.position.x, -camera_pose.position.y, -camera_pose.position.z);
glRotated (carmen_radians_to_degrees (camera_offset.orientation.roll), -1.0, 0.0, 0.0);
glRotated (carmen_radians_to_degrees (camera_offset.orientation.pitch), 0.0, -1.0, 0.0);
glRotated (carmen_radians_to_degrees (camera_offset.orientation.yaw), 0.0, 0.0, -1.0);
glTranslated (-camera_offset.position.x, -camera_offset.position.y, -camera_offset.position.z);
}
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;
}
carmen_velodyne_partial_scan_message
carmen_laser_ldmrs_convert_laser_scan_to_partial_velodyne_message(carmen_laser_ldmrs_message *msg, double laserscan_timestamp)
{
carmen_velodyne_partial_scan_message velodyne_message;
int layer_1_start, layer_1_size, layer_2_start, layer_2_size,
layer_3_start, layer_3_size, layer_4_start, layer_4_size;
get_layers(msg, layer_1_start, layer_1_size, layer_2_start, layer_2_size,
layer_3_start, layer_3_size, layer_4_start, layer_4_size);
double layer_1_start_angle = msg->arraypoints[layer_1_start].horizontal_angle;
double layer_1_end_angle = msg->arraypoints[layer_1_start + layer_1_size - 1].horizontal_angle;
double layer_2_start_angle = msg->arraypoints[layer_2_start].horizontal_angle;
double layer_2_end_angle = msg->arraypoints[layer_2_start + layer_2_size - 1].horizontal_angle;
double layer_3_start_angle = msg->arraypoints[layer_3_start].horizontal_angle;
double layer_3_end_angle = msg->arraypoints[layer_3_start + layer_3_size - 1].horizontal_angle;
double layer_4_start_angle = msg->arraypoints[layer_4_start].horizontal_angle;
double layer_4_end_angle = msg->arraypoints[layer_4_start + layer_4_size - 1].horizontal_angle;
double largest_angle12 = (layer_1_start_angle > layer_2_start_angle) ? layer_1_start_angle: layer_2_start_angle;
double largest_angle34 = (layer_3_start_angle > layer_4_start_angle) ? layer_3_start_angle: layer_4_start_angle;
double largest_angle = (largest_angle12 > largest_angle34) ? largest_angle12: largest_angle34;
double smallest_angle12 = (layer_1_end_angle < layer_2_end_angle) ? layer_1_end_angle: layer_2_end_angle;
double smallest_angle34 = (layer_3_end_angle < layer_4_end_angle) ? layer_3_end_angle: layer_4_end_angle;
double smallest_angle = (smallest_angle12 < smallest_angle34) ? smallest_angle12: smallest_angle34;
double half_a_degree = carmen_degrees_to_radians(0.5);
int number_of_shots = (int) ((largest_angle - smallest_angle) / half_a_degree);
velodyne_message.number_of_32_laser_shots = number_of_shots;
velodyne_message.partial_scan = (carmen_velodyne_32_laser_shot *) calloc (velodyne_message.number_of_32_laser_shots, sizeof(carmen_velodyne_32_laser_shot));
velodyne_message.timestamp = laserscan_timestamp;
velodyne_message.host = carmen_get_host();
for (int i = 0, i1 = 0, i2 = 0, i3 = 0, i4 = 0; i < velodyne_message.number_of_32_laser_shots; i++)
{
for (int j = 0; j < 4; j++)
{
velodyne_message.partial_scan[i].distance[j] = 0;
velodyne_message.partial_scan[i].intensity[j] = 255;
}
double angle = largest_angle - ((double) i) * half_a_degree - 0.000005;
velodyne_message.partial_scan[i].angle = -carmen_radians_to_degrees(angle);
if ((angle <= msg->arraypoints[layer_1_start + i1].horizontal_angle) &&
(angle >= layer_1_end_angle))
{
double distance = msg->arraypoints[layer_1_start + i1].radial_distance;
distance = (distance > 130.0)? 0.0: distance; // larger than 130 meters does no fit an unsigned short int distance field of Velodyne...
velodyne_message.partial_scan[i].distance[0] = (unsigned short) (round(distance * 500.0));
velodyne_message.partial_scan[i].intensity[0] = 255;
i1++;
}
if ((angle <= msg->arraypoints[layer_2_start + i2].horizontal_angle) &&
(angle >= layer_2_end_angle))
{
double distance = msg->arraypoints[layer_2_start + i2].radial_distance;
distance = (distance > 130.0)? 0.0: distance; // larger than 130 meters does no fit an unsigned short int distance field of Velodyne...
velodyne_message.partial_scan[i].distance[1] = (unsigned short) (round(distance * 500.0));
velodyne_message.partial_scan[i].intensity[1] = 255;
i2++;
}
if ((angle <= msg->arraypoints[layer_3_start + i3].horizontal_angle) &&
(angle >= layer_3_end_angle))
{
double distance = msg->arraypoints[layer_3_start + i3].radial_distance;
distance = (distance > 130.0)? 0.0: distance; // larger than 130 meters does no fit an unsigned short int distance field of Velodyne...
velodyne_message.partial_scan[i].distance[2] = (unsigned short) (round(distance * 500.0));
velodyne_message.partial_scan[i].intensity[2] = 255;
i3++;
}
if ((angle <= msg->arraypoints[layer_4_start + i4].horizontal_angle) &&
(angle >= layer_4_end_angle))
{
double distance = msg->arraypoints[layer_4_start + i4].radial_distance;
distance = (distance > 130.0)? 0.0: distance; // larger than 130 meters does no fit an unsigned short int distance field of Velodyne...
velodyne_message.partial_scan[i].distance[3] = (unsigned short) (round(distance * 500.0));
velodyne_message.partial_scan[i].intensity[3] = 255;
i4++;
}
}
return (velodyne_message);
}
void base_odometry_handler(carmen_base_odometry_message *odometry)
{
carmen_warn("%.2f %.2f %.2f\n", odometry->x, odometry->y,
carmen_radians_to_degrees(odometry->theta));
}
void draw_car(CarDrawer* carDrawer)
{
//draw_axis(500.0);
// Car
glPushMatrix();
glTranslatef(carDrawer->car_pose.position.x,carDrawer->car_pose.position.y,carDrawer->car_pose.position.z+carDrawer->car_wheel_diameter/2);
glRotatef(90.0, 1.0, 0.0, 0.0);
glRotatef(0.0, 0.0, 1.0, 0.0);
glColor3f(0.3,0.3,0.3);
//glmDraw(carDrawer->carModel, GLM_SMOOTH | GLM_COLOR);
glmDraw(carDrawer->carModel, GLM_SMOOTH | GLM_COLOR | GLM_TEXTURE);
glPopMatrix();
// Sensor Board
glPushMatrix();
glTranslatef(carDrawer->sensor_board_1_pose.position.x,carDrawer->sensor_board_1_pose.position.y,carDrawer->sensor_board_1_pose.position.z);
glRotatef(carmen_radians_to_degrees(carDrawer->sensor_board_1_pose.orientation.yaw), 0.0f, 0.0f, 1.0f);
glRotatef(carmen_radians_to_degrees(carDrawer->sensor_board_1_pose.orientation.pitch), 0.0f, 1.0f, 0.0f);
glRotatef(carmen_radians_to_degrees(carDrawer->sensor_board_1_pose.orientation.roll), 1.0f, 0.0f, 0.0f);
// Xsens
glPushMatrix();
glTranslatef(carDrawer->xsens_pose.position.x,carDrawer->xsens_pose.position.y,carDrawer->xsens_pose.position.z);
glRotatef(carmen_radians_to_degrees(carDrawer->xsens_pose.orientation.yaw), 0.0f, 0.0f, 1.0f);
glRotatef(carmen_radians_to_degrees(carDrawer->xsens_pose.orientation.pitch), 0.0f, 1.0f, 0.0f);
glRotatef(carmen_radians_to_degrees(carDrawer->xsens_pose.orientation.roll), 1.0f, 0.0f, 0.0f);
glColor3f(1.0,0.6,0.0);
drawBox(carDrawer->xsens_size.x, carDrawer->xsens_size.y, carDrawer->xsens_size.z);
glPopMatrix();
// Laser
glPushMatrix();
glTranslatef(carDrawer->laser_pose.position.x, carDrawer->laser_pose.position.y, carDrawer->laser_pose.position.z);
glRotatef(carmen_radians_to_degrees(carDrawer->laser_pose.orientation.yaw), 0.0f, 0.0f, 1.0f);
glRotatef(carmen_radians_to_degrees(carDrawer->laser_pose.orientation.pitch), 0.0f, 1.0f, 0.0f);
glRotatef(carmen_radians_to_degrees(carDrawer->laser_pose.orientation.roll), 1.0f, 0.0f, 0.0f);
glColor3f(0.0,0.0,1.0);
drawBox(carDrawer->laser_size.x, carDrawer->laser_size.y, carDrawer->laser_size.z);
glPopMatrix();
glPopMatrix();
/*
glPushMatrix();
glPushMatrix();
glColor3f(0.3,0.3,0.3);
draw_wheel_axis(carDrawer->car_wheel_diameter,carDrawer->car_size.y);
glPushMatrix();
glTranslatef(carDrawer->car_axis_distance, 0.0, 0.0);
draw_wheel_axis(carDrawer->car_wheel_diameter,carDrawer->car_size.y);
glPopMatrix();
glColor3f(1.0,0.0,0.0);
glPushMatrix();
glTranslatef(carDrawer->car_pose.position.x,carDrawer->car_pose.position.y,carDrawer->car_pose.position.z+carDrawer->car_wheel_diameter/2);
glRotatef(carDrawer->car_pose.orientation.roll, 1.0f, 0.0f, 0.0f);
glRotatef(carDrawer->car_pose.orientation.pitch, 0.0f, 1.0f, 0.0f);
glRotatef(carDrawer->car_pose.orientation.yaw, 0.0f, 0.0f, 1.0f);
drawBox(carDrawer->car_size.x, carDrawer->car_size.y, carDrawer->car_size.z-carDrawer->car_wheel_diameter);
glPopMatrix();
glPopMatrix();
glPopMatrix();
*/
}
static void info(int argc, char *argv[])
{
time_t creation_time;
char *filename;
char username[100], origin[100], description[100];
int chunk_size, i;
carmen_map_config_t config;
carmen_map_placelist_t place_list;
carmen_offlimits_t *offlimits_list;
int list_length;
carmen_global_offset_t offset;
/* Check for the appropriate command line argument */
if(argc != 3)
carmen_die("Error in info: wrong number of parameters.\n");
filename = argv[2];
/* Make sure the file exists */
if (!carmen_map_file(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"
"Do you have an incompatible file?\n"
"Current map version: %s\n", filename, CARMEN_MAP_VERSION);
/* Print the CREATOR information if the chunk exists */
if(carmen_map_read_creator_chunk(filename, &creation_time,
username, origin, description) == 0) {
printf("\nMap CREATOR information:\n");
printf("------------------------\n");
printf("Map author : %s\n", username);
printf("Creation date : %s", asctime(localtime(&creation_time)));
printf("Origin : %s\n", origin);
printf("Description : %s", description);
printf("\n");
}
/* Print the list of chunks in the file */
printf("Map contains the following CHUNK types:\n");
printf("---------------------------------------\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_CREATOR_CHUNK);
if(chunk_size > 0)
printf("CREATOR : yes (%d bytes)\n", chunk_size);
else
printf("CREATOR : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_GRIDMAP_CHUNK);
if(chunk_size > 0)
{
carmen_map_read_gridmap_config(filename, &config);
printf("GRIDMAP : yes size : %4d x %4d, resolution : %.2f m/cell (%d bytes)\n",
config.x_size, config.y_size, config.resolution, chunk_size);
}
else
printf("GRIDMAP : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_OFFLIMITS_CHUNK);
if(chunk_size > 0) {
printf("OFFLIMITS : yes (%d bytes)\n", chunk_size);
carmen_map_read_offlimits_chunk(filename, &offlimits_list, &list_length);
for (i = 0; i < list_length; i++) {
if (offlimits_list[i].type == CARMEN_OFFLIMITS_POINT_ID)
carmen_warn("\tOfflimits %d: point: %d %d\n", i, offlimits_list[i].x1,
offlimits_list[i].y1);
else if (offlimits_list[i].type == CARMEN_OFFLIMITS_LINE_ID)
carmen_warn("\tOfflimits %d: line: %d %d %d %d\n", i, offlimits_list[i].x1,
offlimits_list[i].y1, offlimits_list[i].x2, offlimits_list[i].y2);
else if (offlimits_list[i].type == CARMEN_OFFLIMITS_RECT_ID)
carmen_warn("\tOfflimits %d: rect: %d %d %d %d\n", i, offlimits_list[i].x1,
offlimits_list[i].y1, offlimits_list[i].x2, offlimits_list[i].y2);
}
}
else
printf("OFFLIMITS : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_PLACES_CHUNK);
if(chunk_size > 0)
printf("PLACES : yes (%d bytes)\n", chunk_size);
else
printf("PLACES : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_EXPECTED_CHUNK);
if(chunk_size > 0)
printf("EXPECTED : yes (%d bytes)\n", chunk_size);
else
printf("EXPECTED : no\n");
chunk_size = carmen_map_chunk_exists(filename, CARMEN_MAP_LASERSCANS_CHUNK);
if(chunk_size > 0)
printf("LASERSCANS : yes (%d bytes)\n", chunk_size);
else
printf("LASERSCANS : no\n");
if(carmen_map_chunk_exists(filename, CARMEN_MAP_PLACES_CHUNK)) {
printf("\nMap contains the following places:\n");
printf("----------------------------------\n");
carmen_map_read_places_chunk(filename, &place_list);
for(i = 0; i < place_list.num_places; i++)
if(place_list.places[i].type == CARMEN_NAMED_POSITION_TYPE)
printf("%-20s (%.1f m, %.1f m)\n", place_list.places[i].name,
place_list.places[i].x, place_list.places[i].y);
else if(place_list.places[i].type == CARMEN_NAMED_POSE_TYPE)
printf("%-20s (%.1f m, %.1f m, %.1f deg)\n",
place_list.places[i].name, place_list.places[i].x,
place_list.places[i].y,
carmen_radians_to_degrees(place_list.places[i].theta));
else if(place_list.places[i].type == CARMEN_LOCALIZATION_INIT_TYPE)
printf("%-20s (%.1f m, %.1f m, %.1f deg) std = (%.2f m, %.2f m, "
"%.2f deg)\n", place_list.places[i].name,
place_list.places[i].x, place_list.places[i].y,
carmen_radians_to_degrees(place_list.places[i].theta),
place_list.places[i].x_std, place_list.places[i].y_std,
carmen_radians_to_degrees(place_list.places[i].theta_std));
}
chunk_size = carmen_map_chunk_exists(filename,
CARMEN_MAP_GLOBAL_OFFSET_CHUNK);
if (chunk_size > 0) {
carmen_map_read_global_offset_chunk(filename, &offset);
printf("OFFSET : %10.3f %10.3f %10.3f rads\n", offset.x,
offset.y, offset.theta);
} else
printf("OFFSET : no\n");
}
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
carmen_planner_util_test_trajectory(carmen_planner_path_p path)
{
int index;
carmen_ackerman_traj_point_t point = {0,0,0,0,0};
carmen_ackerman_traj_point_p point_p;
carmen_planner_util_clear_path(path);
if (path->length != 0)
carmen_die("Test failed: clear should set length to 0, but length is %d\n",
path->length);
memset(&point, 0, sizeof(carmen_ackerman_traj_point_t));
for (index = 0; index < 100; index++)
{
point.x = index;
point.y = index;
carmen_planner_util_add_path_point(point, path);
}
for (index = 0; index < 100; index++)
{
point_p = carmen_planner_util_get_path_point(index, path);
if (point_p->x != index || point_p->y != index)
carmen_die("Test failed: After 100 set points, get point on %d did not "
"match: was %.0f %.0f\n", index, point_p->x, point_p->y);
}
point.x = 50.5;
point.y = 50.5;
carmen_planner_util_insert_blank(50, path);
carmen_planner_util_set_path_point(50, &point, path);
point_p = carmen_planner_util_get_path_point(50, path);
if (fabs(point_p->x - 50.5) > 0.05 || fabs(point_p->y - 50.5) > 0.05)
carmen_die("Blank then set failed.\n");
if (path->length != 101)
carmen_die("Length (%d) not 101 after insert_blank then set.\n",
path->length);
point.x = 60.5;
point.y = 60.5;
carmen_planner_util_insert_path_point(60, &point, path);
if (fabs(point_p->x - 50.5) > 0.05 || fabs(point_p->y - 50.5) > 0.05)
carmen_die("Blank then set failed.\n");
if (path->length != 102)
carmen_die("Length (%d) not 102 after insert_blank then set.\n",
path->length);
carmen_planner_util_delete_path_point(50, path);
carmen_planner_util_delete_path_point(60, path);
if (path->length != 100)
carmen_die("Length (%d) not 100 after insert_blank then set.\n",
path->length);
carmen_planner_util_clip_path(50, path);
for (index = 0; index < path->length; index++)
carmen_planner_util_delete_path_point(index, path);
for (index = 0; index < path->length; index++)
{
point_p = carmen_planner_util_get_path_point(index, path);
if (point_p->x != 2*index+1 || point_p->y != 2*index+1)
carmen_die("Test failed: After deleting even points, get point on %d "
"(%d) did not match: was %.0f %.0f %.0f\n", 2*index+1, index,
point_p->x, point_p->y,
carmen_radians_to_degrees(point_p->theta));
}
}
void
publish_rrt_path_message(list<RRT_Path_Edge> &path)
{
int i = 0;
rrt_path_message msg;
list<RRT_Path_Edge>::iterator it;
msg.host = carmen_get_host();
msg.timestamp = GlobalState::rrt_planner_timestamp;
msg.last_goal = GlobalState::last_goal ? 1 : 0;
if (GlobalState::goal_pose)
{
msg.goal.x = GlobalState::goal_pose->x;
msg.goal.y = GlobalState::goal_pose->y;
msg.goal.theta = GlobalState::goal_pose->theta;
}
else
{
msg.goal.x = msg.goal.y = msg.goal.theta = 0.0;
}
if (path.empty())
{
// return;
msg.size = 0;
msg.path = NULL;
}
else
{
msg.size = path.size();
msg.path = (Edge_Struct *) malloc(sizeof(Edge_Struct) * msg.size);
}
for (it = path.begin(); it != path.end(); it++, i++)
{
msg.path[i].p1.x = it->p1.pose.x;
msg.path[i].p1.y = it->p1.pose.y;
msg.path[i].p1.theta = it->p1.pose.theta;
msg.path[i].p1.v = it->p1.v_and_phi.v;
msg.path[i].p1.phi = it->p1.v_and_phi.phi;
msg.path[i].p2.x = it->p2.pose.x;
msg.path[i].p2.y = it->p2.pose.y;
msg.path[i].p2.theta = it->p2.pose.theta;
msg.path[i].p2.v = it->p2.v_and_phi.v;
msg.path[i].p2.phi = it->p2.v_and_phi.phi;
msg.path[i].v = it->command.v;
msg.path[i].phi = it->command.phi;
msg.path[i].time = it->time;
// printf( "p1.x = %lf, p1.y = %lf, p1.theta = %lf, p1.v = %lf, p1.phi = %lf\n"
// "p2.x = %lf, p2.y = %lf, p2.theta = %lf, p2.v = %lf, p2.phi = %lf\n"
// "command.v = %lf, command.phi = %lf, command.time = %lf\n",
// msg.path[i].p1.x, msg.path[i].p1.y, msg.path[i].p1.theta, msg.path[i].p1.v, msg.path[i].p1.phi,
// msg.path[i].p2.x, msg.path[i].p2.y, msg.path[i].p2.theta, msg.path[i].p2.v, msg.path[i].p2.phi,
// msg.path[i].v, msg.path[i].phi, msg.path[i].time);
if (GlobalState::show_debug_info)
printf("v = %2.2lf, phi = %2.2lf, t = %2.3lf, p1.v = %2.2lf, p1.phi = %2.2lf, p2.v = %2.2lf, p2.phi = %2.2lf\n",
it->command.v, carmen_radians_to_degrees(it->command.phi), it->time,
it->p1.v_and_phi.v, carmen_radians_to_degrees(it->p1.v_and_phi.phi),
it->p2.v_and_phi.v, carmen_radians_to_degrees(it->p2.v_and_phi.phi));
}
if (GlobalState::show_debug_info)
{
printf("\n");
fflush(stdout);
}
Publisher_Util::publish_rrt_path_message(&msg);
free(msg.path);
}