void
carmen_velodyne_variable_scan_update_points(carmen_velodyne_variable_scan_message *message,
int vertical_resolution, spherical_point_cloud *points, unsigned char *intensity,
int *ray_order, double *vertical_correction, float range_max, double timestamp)
{
int i, j;
points->timestamp = timestamp;
for (i = 0; i < message->number_of_shots; i++)
{
for (j = 0; j < vertical_resolution; j++)
{
float angle = carmen_degrees_to_radians(message->partial_scan[i].angle);
float range = message->partial_scan[i].distance[ray_order[j]] / 500.0;
if (range <= 0.0) // @@@ Aparentemente o Velodyne diz range zero quando de range_max
range = range_max;
intensity[i * vertical_resolution + j] = message->partial_scan[i].intensity[j];
points->sphere_points[i * vertical_resolution + j].horizontal_angle = carmen_normalize_theta(-angle);
points->sphere_points[i * vertical_resolution + j].vertical_angle = carmen_degrees_to_radians(vertical_correction[j]);
points->sphere_points[i * vertical_resolution + j].length = range;
}
}
}
static void
build_points_vector_handler()
{
static int firstime = 1;
if (firstime)
{
points_vector[0].x = globalpos.globalpos.x;
points_vector[0].y = globalpos.globalpos.y;
points_vector[0].theta = globalpos.globalpos.theta;
points_vector_size = 1;
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(10), 3.0, &car_config);
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(-10), 3.0, &car_config);
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(0), 3.0, &car_config);
printf("enviando %d, pose %f %f %f\n", points_vector_size, globalpos.globalpos.x, globalpos.globalpos.y, globalpos.globalpos.theta);
carmen_motion_planner_publish_path_message(points_vector, points_vector_size, 0);
// publish_navigator_ackerman_plan_message(points_vector, points_vector_size);
// send_base_ackerman_command(points_vector, points_vector_size);
carmen_ipc_sleep(0.5);
firstime = 0;
}
}
void
Robot::drawEllipse()
{
glColor3f(0.6, 0.34, 0.8);
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));
glPushMatrix();
glTranslatef(robot_pose.getOrigin().x(),robot_pose.getOrigin().y(), robot_pose.getOrigin().z());
glRotatef(this->angle_, 0.0f, 0.0f, 1.0f);
glBegin(GL_LINE_LOOP);
for(int i=0; i < 360; i++)
{
//convert degrees into radians
float degInRad = carmen_degrees_to_radians((double)i);
glVertex2f(cos(degInRad)*this->minor_axis_,sin(degInRad)*this->major_axis_);
}
glEnd();
glPopMatrix();
}
void
NFQ_train(struct fann *qlearning_ann, int num_data, state *s, action *a, carmen_simulator_ackerman_config_t *simulator_config)
{
int k = 0; int b; int N = num_data - 1;
double gamma = 0.3, target;
static struct fann_train_data *ann_data;
static double max_atan_curvature;
if (ann_data == NULL)
{
initialize_ann_data(&ann_data, N*3, 4, 1);
max_atan_curvature = atan(compute_curvature(carmen_degrees_to_radians(30.0), simulator_config));
}
do
{
target = c(s[k], a[k], s[k+1]) + gamma * min_b_Q(&b, qlearning_ann, s[k+1], a[k+1], simulator_config);
ann_data->input[k][0] = s[k].atan_desired_curvature / max_atan_curvature;
ann_data->input[k][1] = s[k].atan_current_curvature / max_atan_curvature;
ann_data->input[k][2] = (double) s[k].b / 6.0;
ann_data->input[k][3] = a[k].steering_command / 100.0;
ann_data->output[k][0] = target;
//fprintf(stdout, "[%d] d: %.5lf c: %.5lf b: %.5lf (%d') s: %.5lf t: %.5lf\n",
// k, ann_data->input[k][0], ann_data->input[k][1], ann_data->input[k][2]*10, b, ann_data->input[k][3]*100, ann_data->output[k][0]);
k++;
} while(k < N);
//(ann, data, max_epochs, epochs_between_reports, desired_error)
fann_train_on_data(qlearning_ann, ann_data, 200, 0, 0.001);
}
velodyne_intensity_drawer* create_velodyne_intensity_drawer(carmen_pose_3D_t velodyne_pose, carmen_pose_3D_t sensor_board_pose)
{
velodyne_intensity_drawer* v_drawer = malloc(sizeof(velodyne_intensity_drawer));
v_drawer->pcloud_drawer = create_point_cloud_drawer(10000);
v_drawer->velodyne_pose = velodyne_pose;
v_drawer->sensor_board_pose = sensor_board_pose;
v_drawer->horizontal_angle = carmen_degrees_to_radians(0.0);
v_drawer->horizontal_range = carmen_degrees_to_radians(45.0);
v_drawer->vertical_position = 18;
return v_drawer;
}
void
initGl ()
{
int zero = 0;
glutInit (&zero, NULL);
glewInit ();
glClearColor (0.0f, 0.0f, 0.0f, 0.0f);
glClearDepth (1.0);
glDepthFunc (GL_LESS);
glEnable (GL_DEPTH_TEST);
glShadeModel (GL_SMOOTH);
GLfloat light_ambient[] = {0.5f, 0.5f, 0.5f, 1.0f};
GLfloat light_diffuse[] = {0.4f, 0.4f, 0.3f, 1.0f};
GLfloat light_position[] = {0.0f, 1.0f, 2.0f, 1.0f};
glLightfv (GL_LIGHT1, GL_AMBIENT, light_ambient);
glLightfv (GL_LIGHT1, GL_DIFFUSE, light_diffuse);
glLightfv (GL_LIGHT1, GL_POSITION, light_position);
glEnable (GL_LIGHT1);
glEnable (GL_LIGHTING);
glColorMaterial (GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glEnable (GL_COLOR_MATERIAL);
map_image_texture_id = create_texture ();
glGenBuffers (1, &laser_buffer_id);
glBindBuffer (GL_ARRAY_BUFFER, laser_buffer_id);
laser_pos_buffer = NULL;
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
gluPerspective (45.0f, WINDOW_WIDTH / WINDOW_HEIGHT, 0.1f, 4000.0f); // Calculate The Aspect Ratio Of The Window
carmen_pose_3D_t zero_pose;
zero_pose.position.x = 0.0;
zero_pose.position.y = 0.0;
zero_pose.position.z = 0.0;
zero_pose.orientation.roll = 0.0;
zero_pose.orientation.pitch = 0.0;
zero_pose.orientation.yaw = 0.0;
camera_pose = zero_pose;
camera_offset = zero_pose;
camera_pose.position.z = 30.0;
camera_pose.orientation.pitch = carmen_degrees_to_radians (90.0);
background_r = 0.0;
background_g = 0.0;
background_b = 0.0;
glMatrixMode (GL_MODELVIEW);
}
void
init_message_laser_params(int laser_id, double accuracy, double fov, double resolution, double maxrange)
{
fov_in_degrees = fov;
message_laser.id = laser_id;
message_laser.num_remissions=0;
message_laser.config.remission_mode = REMISSION_NONE;
message_laser.config.laser_type = LASER_EMULATED_USING_KINECT;
message_laser.config.fov = carmen_degrees_to_radians(fov);
message_laser.config.start_angle = -0.5 * message_laser.config.fov;
message_laser.config.angular_resolution = carmen_degrees_to_radians(resolution);
message_laser.num_readings=1 + carmen_round(message_laser.config.fov / message_laser.config.angular_resolution);
message_laser.config.maximum_range = maxrange;
message_laser.config.accuracy = accuracy;
message_laser.host = carmen_get_host();
message_laser.range = laser_ranges;
}
void Velodyne::SetupRotationalAndVerticalTables()
{
// Set up cached values for sin and cos of all the possible headings
for (unsigned short rot_index = 0; rot_index < 36000; ++rot_index)
{
float rotation = carmen_degrees_to_radians(0.01 * rot_index);
cos_rot_table[rot_index] = cosf(rotation);
sin_rot_table[rot_index] = sinf(rotation);
}
for (unsigned short vert_index = 0; vert_index < 32; ++vert_index)
{
float vert_radian = carmen_degrees_to_radians(vertical_correction[vert_index]);
cos_vert_table[vert_index] = cosf(vert_radian);
sin_vert_table[vert_index] = sinf(vert_radian);
}
}
void Carmen_Thread::set_localize_position(carmen_point_t pose) {
carmen_point_t std = (carmen_point_t) {
0.2, 0.2, carmen_degrees_to_radians(0.0)/*, -pose.theta*/
};
carmen_localize_ackerman_initialize_gaussian_command(pose, std);
carmen_localize_initialize_gaussian_command(pose, std);
}
static void set_robot_pose(carmen_point_t pose) {
carmen_point_t std = {0.2, 0.2, carmen_degrees_to_radians(4.0)};
carmen_localize_initialize_gaussian_command(pose, std);
if (simulation)
carmen_simulator_set_truepose(&pose);
global_pos_reset = 1;
}
LaserConfig::LaserConfig() {
setLaserType(UMKNOWN_PROXIMITY_SENSOR);
setStartAngle(-0.5*M_PI);
setFOV(M_PI);
setAngularResolution(carmen_degrees_to_radians(1.0));
setMaximumRange(81.9);
setAccuracy(0.01);
setRemissionMode(REMISSION_NONE);
}
void Mapper::addKinectData(carmen_kinect_depth_message* scan, const btTransform& w2l, double ahfov, int sample)
{
boost::mutex::scoped_lock(map_.mutex_);
// position of the laser in the world coordinates
btVector3 origin = w2l * btVector3(0.0, 0.0, 0.0);
unsigned int num_readings_x = scan->width;
unsigned int num_readings_y = scan->height;
double hfov = ahfov;
double vfov = hfov * ((double)scan->height/(double)scan->width);
//double scan_step_x = hfov/((double)scan->width);
//double scan_step_y = vfov/((double)scan->height);
double scan_angle_y = 0.0;
double scan_angle_x = 0.0;
double focal_length_in_pixels = ((double)scan->width ) / (2.0 * tan(carmen_degrees_to_radians(hfov/ 2.0)));
for(size_t j=0; j < num_readings_y; j+=3*sample)
{
//scan_angle_x = hfov/2.0;
scan_angle_y = -atan((j - (((double)scan->height)/2.0))/focal_length_in_pixels);
for (size_t i = 0; i < num_readings_x; i+=4*sample)
{
scan_angle_x = -atan((i - (((double)scan->width)/2.0))/focal_length_in_pixels);
int k = j * num_readings_x + i;
if (scan->depth[k] > SCAN_RANGE_MIN && scan->depth[k] < SCAN_RANGE_MAX)
{
// valid, in range reading
btVector3 obstacle = w2l * btVector3(cos(scan_angle_x)*scan->depth[k]/cos(scan_angle_x), sin(scan_angle_x)*scan->depth[k]/cos(scan_angle_x), sin(scan_angle_y)*scan->depth[k]/cos(scan_angle_y));
addBeamReading(origin, obstacle);
}
else if (scan->depth[k] > SCAN_RANGE_MAX || scan->depth[k] == 0)
{
// out of range reading
}
else
{
// invalid reading - too close, or error
}
//printf("sax: %6.2f, say: %6.2f\n", scan_angle_x, scan_angle_y);
// increment scan angle
//scan_angle_x -= scan_step_x*sample;
}
//scan_angle_y -= scan_step_y*sample;
}
}
void add_velodyne_message(velodyne_360_drawer* v_drawer, carmen_velodyne_partial_scan_message* velodyne_message)
{
static double vertical_correction[32] = { -30.67, -9.3299999, -29.33, -8.0, -28.0, -6.6700001, -26.67, -5.3299999, -25.33, -4.0,
-24.0, -2.6700001, -22.67, -1.33, -21.33, 0.0, -20.0, 1.33, -18.67, 2.6700001, -17.33, 4.0, -16.0, 5.3299999, -14.67, 6.6700001,
-13.33, 8.0, -12.0, 9.3299999, -10.67, 10.67 };
switch_current_last(v_drawer);
int num_points = velodyne_message->number_of_32_laser_shots*32;
v_drawer->num_points_current = num_points;
v_drawer->points_current = realloc(v_drawer->points_current, num_points*sizeof(carmen_vector_3D_t));
v_drawer->angles_current = realloc(v_drawer->angles_current, num_points*sizeof(double));
rotation_matrix* velodyne_to_board_matrix = create_rotation_matrix(v_drawer->velodyne_pose.orientation);
rotation_matrix* board_to_car_matrix = create_rotation_matrix(v_drawer->sensor_board_pose.orientation);
int i;
for(i = 0; i < velodyne_message->number_of_32_laser_shots; i++)
{
double rot_angle = carmen_degrees_to_radians(velodyne_message->partial_scan[i].angle);
int j;
for(j = 0; j < 32; j++)
{
double vert_angle = carmen_degrees_to_radians(vertical_correction[j]);
carmen_vector_3D_t point_position = get_velodyne_point_car_reference( -rot_angle, vert_angle,
velodyne_message->partial_scan[i].distance[j]/500.0,
v_drawer->velodyne_pose, v_drawer->sensor_board_pose,
velodyne_to_board_matrix, board_to_car_matrix);
v_drawer->points_current[i*32 + j] = point_position;
v_drawer->angles_current[i*32 + j] = rot_angle;
}
}
destroy_rotation_matrix(velodyne_to_board_matrix);
destroy_rotation_matrix(board_to_car_matrix);
}
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));
}
int main(int argc, char **argv)
{
float vel, acc;
if(argc < 3)
carmen_die("usage: %s <rotation speed> <acceleration>\n", argv[0]);
vel = 0.5 * carmen_degrees_to_radians(atof(argv[1])) * ROT_VEL_FACT_RAD;
acc = atof(argv[2])/METRES_TO_SCOUT;
connect_robot(1, MODEL_SCOUT2, "/dev/ttyS0", 38400);
ac(acc, acc, 0);
carmen_warn("rv: %f makes vel: %f\n", carmen_degrees_to_radians(atof(argv[1])), vel);
while(1) {
vm(vel, -vel, 0);
usleep(250000);
}
return 0;
}
Pose Util::random_pose()
{
Pose p;
p.x = rand() % GlobalState::cost_map.config.x_size;
p.y = rand() % GlobalState::cost_map.config.y_size;
p.theta = carmen_normalize_theta(carmen_degrees_to_radians((rand() % 360) - 180));
p = Util::to_global_pose(p);
return p;
}
bool Util::can_reach(Pose robot, Pose goal)
{
double theta_diff;
bool goal_reachable;
theta_diff = goal.theta - robot.theta;
goal_reachable = fabs(theta_diff) < carmen_degrees_to_radians(70);
//printf("%f %f %d\n", carmen_radians_to_degrees(theta_diff), heading(robot, goal), goal_reachable);
return goal_reachable;
}
carmen_point_t
publish_starting_pose(carmen_point_t pose)
{
carmen_point_t std;
std.x = 0.001;
std.y = 0.001;
std.theta = carmen_degrees_to_radians(0.01);
carmen_localize_ackerman_initialize_gaussian_command(pose, std);
return pose;
}
void
convert_depthmap_to_beams(double* beams, float fov_horiz_in_degrees, int number_of_angular_steps, float max_range,
float* depthmap, int depthmap_width, int depthmap_height, int depthmap_size)
{
int j, i = number_of_angular_steps-1;
float alpha, alpha_step, z;
float focal_length_in_pixels = ((float)depthmap_width ) / (2.0 * tan(carmen_degrees_to_radians(fov_horiz_in_degrees/ 2.0)));
int x, y = depthmap_height/2;
alpha_step = fov_horiz_in_degrees / (float) number_of_angular_steps;
for (alpha = -fov_horiz_in_degrees/2.0; alpha < fov_horiz_in_degrees/2.0; alpha += alpha_step, i--)
{
float alpha_in_radians = carmen_degrees_to_radians(alpha);
float tan_alpha_in_radians = tan(alpha_in_radians);
float cos_alpha_in_radians = cos(alpha_in_radians);
x = (int)(((float)depthmap_width)/2.0) + (int)floor(focal_length_in_pixels * tan_alpha_in_radians);
j = y * depthmap_width + x;
if (j < depthmap_size)
z = depthmap[j];
else
{
carmen_warn("Laser beams range greater than depthmap size.");
continue;
}
if (z < 0.5)
z = max_range;
if (fabs(cos_alpha_in_radians) > 1e-5)
beams[i] = z / cos_alpha_in_radians;
else
beams[i] = z;
}
if(i>0)
carmen_warn("Laser beams does not fulfill entirely FOV: total range %d, not filled range %d.\n", number_of_angular_steps, i);
}
int
main(int argc, char **argv)
{
rrt_parking = new RRT_Parking();
rrt_lane = new RRT_Lane();
selected_rrt = rrt_lane;
rrt_lane->set_change_path_condition(0.8, carmen_degrees_to_radians(8));
rrt_lane->set_stop_condition(0.8, carmen_degrees_to_radians(30));
rrt_parking->set_change_path_condition(0.8, carmen_degrees_to_radians(8));
rrt_parking->set_stop_condition(1.0, carmen_degrees_to_radians(10));
rrt_parking->set_max_distance_grad(12.0);
rrt_parking->set_distance_near(12.0);
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
read_parameters(argc, argv);
Ackerman::init_search_parameters();
rs_init_parameters(GlobalState::robot_config.max_phi, GlobalState::robot_config.distance_between_front_and_rear_axles);
GlobalState::lane_points = Lane::get_street(GlobalState::rddf_path);
RRT_IPC::register_handlers();
printf("===============RRT Parameters===============\n");
printf("Timeout: %f s\n", GlobalState::timeout);
printf("Max distance interval %f m\n", GlobalState::distance_interval);
printf("Use pose from simulator: %s\n", GlobalState::cheat ? "Yes" : "No");
carmen_ipc_dispatch();
return 0;
}
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
localize_ackerman_velodyne_laser_read_parameters(int argc, char **argv)
{
static char frontlaser_fov_string[256];
static char frontlaser_res_string[256];
carmen_param_t param_list[] =
{
{(char *)"robot", (char*)"frontlaser_id", CARMEN_PARAM_INT, &(front_laser_config.id), 0, NULL}
};
carmen_param_install_params(argc, argv, param_list, sizeof(param_list) / sizeof(param_list[0]));
sprintf(frontlaser_fov_string, (char*)"laser%d_fov", front_laser_config.id);
sprintf(frontlaser_res_string, (char*)"laser%d_resolution", front_laser_config.id);
carmen_param_t param_list_front_laser[] =
{
{(char *)"simulator", (char*)"frontlaser_maxrange", CARMEN_PARAM_DOUBLE, &(front_laser_config.max_range), 1, NULL},
{(char *)"robot", (char*)"frontlaser_offset", CARMEN_PARAM_DOUBLE, &(front_laser_config.offset), 1, NULL},
{(char *)"robot", (char*)"frontlaser_side_offset", CARMEN_PARAM_DOUBLE, &(front_laser_config.side_offset), 1, NULL},
{(char *)"robot", (char*)"frontlaser_angular_offset", CARMEN_PARAM_DOUBLE, &(front_laser_config.angular_offset), 1, NULL},
{(char *)"laser", frontlaser_fov_string, CARMEN_PARAM_DOUBLE, &(front_laser_config.fov), 0, NULL},
{(char *)"laser", frontlaser_res_string, CARMEN_PARAM_DOUBLE, &(front_laser_config.angular_resolution), 0, NULL},
};
carmen_param_install_params(argc, argv, param_list_front_laser, sizeof(param_list_front_laser) / sizeof(param_list_front_laser[0]));
front_laser_config.angular_resolution = carmen_degrees_to_radians(front_laser_config.angular_resolution);
front_laser_config.fov = carmen_degrees_to_radians(front_laser_config.fov);
fill_laser_config_data(&front_laser_config);
localize_ackerman_velodyne_laser_initialize();
}
static int
next_waypoint_astar(carmen_ackerman_traj_point_p waypoint)
{
if (path.path == NULL)
return -1;
// int i;
// carmen_ackerman_traj_point_t pose = *waypoint;
// double min_pose_dist = carmen_distance_ackerman_traj(waypoint, &path.path[0]);
// double pose_dist = 0;
// double temp;
// for (i = 1; i <= 30; i++)
// {
// pose_dist = carmen_distance_ackerman_traj(&pose, &path.path[0]);
// if (pose_dist < 0.1 || pose_dist > min_pose_dist)
// {
// path.path[0].v = (path.path[0].v / fabs(path.path[0].v)) * (i / 20.0);
// break;
// }
// printf("pose_dist %f x %f y %f\n",pose_dist, pose.x, pose.y);
// min_pose_dist = pose_dist;
// pose = predict_new_robot_position(*waypoint, 1, path.path[0].phi, (i / 20.0), &carmen_robot_ackerman_config);
// }
// printf("v %f d_p %f i %d %f\n",path.path[0].v, carmen_distance_ackerman_traj(waypoint, &path.path[0]), i, temp);
double delta_theta = fabs(carmen_normalize_theta(waypoint->theta - path.path[0].theta));
replan = 0;
if (carmen_distance_ackerman_traj(waypoint, &path.path[0]) <= 0.3 && delta_theta < carmen_degrees_to_radians(1.5))
{
delete_path_point(0);
min_delta_d = INTMAX_MAX;
if (path.path_size <= 1)
{
replan = 1;
return 1;
}
}
if (min_delta_d != -1 && carmen_distance_ackerman_traj(waypoint, &path.path[0]) - min_delta_d > 0.5)
{
replan = 1;
return -1;
}
min_delta_d = carmen_fmin(carmen_distance_ackerman_traj(waypoint, &path.path[0]), min_delta_d);
return 0;
}
static void get_street_points(vector<carmen_point_t> &carmen_poses,
vector<carmen_point_t> &street_points)
{
carmen_point_t last_point;
double distance;
double theta, last_theta;
bool theta_initialized;
if (carmen_poses.empty())
return;
last_point = carmen_poses.front();
street_points.push_back(last_point);
theta_initialized = false;
for (unsigned int i = 0; i < carmen_poses.size(); i++)
{
distance = carmen_distance(&last_point, &carmen_poses[i]);
if (distance > 1.0)
{
if (!theta_initialized)
{
last_theta = carmen_normalize_theta(
atan2(carmen_poses[i].y - last_point.y,
carmen_poses[i].x - last_point.x));
theta_initialized = true;
continue;
}
theta = carmen_normalize_theta(
atan2(carmen_poses[i].y - last_point.y,
carmen_poses[i].x - last_point.x));
if (fabs(carmen_normalize_theta(last_theta - theta))
> carmen_degrees_to_radians(0.25) || distance > 30.0)
{
last_point = carmen_poses[i];
street_points.push_back(last_point);
theta_initialized = false;
}
}
}
}
static void
detect_gps_performance_change(carmen_gps_gphdt_message *message)
{
static double previous_heading = 0.0;
if (!xsens_handler.initial_state_initialized)
{
previous_heading = message->heading;
return;
}
if ((xsens_handler.gps_hdt.valid != message->valid) ||
(fabs(previous_heading - message->heading)) > carmen_degrees_to_radians(15.0))
xsens_handler.gps_orientation_performance_changed = 1;
previous_heading = message->heading;
}
static void test_transforms()
{
double PI = carmen_degrees_to_radians(180);
tf::Transform t1;
t1.setOrigin(tf::Vector3(1.0, 0.0, 0.0));
t1.setRotation(tf::Quaternion(PI/2, 0.0, 0.0));
tf::Transform t2;
t2.setOrigin(tf::Vector3(1.0, 0.0, 0.0));
t2.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
tf::Transform t3 = t1*t2;
print_transform(t1);
print_transform(t2);
print_transform(t3);
}
double
run_qlearning_network(struct fann *qlearning_ann,
double atan_desired_curvature, double atan_current_curvature, double steering_command, double b, carmen_simulator_ackerman_config_t *simulator_config)
{
fann_type ann_input[4], *ann_output;
static double max_atan_curvature;
static int first_time = 1;
if (first_time)
{
max_atan_curvature = atan(compute_curvature(carmen_degrees_to_radians(30.0), simulator_config));
first_time = 0;
}
ann_input[0] = atan_desired_curvature / max_atan_curvature;
ann_input[1] = atan_current_curvature / max_atan_curvature;
ann_input[2] = b / 6.0;
ann_input[3] = steering_command / 100.0;
ann_output = fann_run(qlearning_ann, ann_input);
return ann_output[0];
}
double
compute_new_phi_with_ann_new(carmen_simulator_ackerman_config_t *simulator_config)
{
static double steering_command = 0.0;
double atan_current_curvature, atan_desired_curvature;
static fann_type steering_ann_input[NUM_STEERING_ANN_INPUTS];
fann_type *steering_ann_output __attribute__ ((unused));
static struct fann *steering_ann = NULL;
if (steering_ann == NULL)
{
steering_ann = fann_create_from_file("steering_ann.net");
if (steering_ann == NULL)
{
printf("Error: Could not create steering_ann\n");
exit(1);
}
init_steering_ann_input(steering_ann_input);
}
atan_current_curvature = atan(compute_curvature(simulator_config->phi, simulator_config));
atan_desired_curvature = atan(compute_curvature(simulator_config->target_phi, simulator_config));
compute_steering_with_qlearning(&steering_command,
atan_desired_curvature, atan_current_curvature, simulator_config->delta_t, simulator_config);
build_steering_ann_input(steering_ann_input, steering_command, atan_current_curvature);
steering_ann_output = fann_run(steering_ann, steering_ann_input);
// Alberto: O ganho de 1.05 abaixo foi necessario pois a rede nao estava gerando curvaturas mais extremas
// que nao aparecem no treino, mas apenas rodando livremente na simulacao
//simulator_config->phi = 1.05 * get_phi_from_curvature(tan(steering_ann_output[0]), simulator_config);
simulator_config->phi += steering_command / 500.0;
simulator_config->phi = carmen_clamp(carmen_degrees_to_radians(-30.0), simulator_config->phi , carmen_degrees_to_radians(30.0));
if (simulator_config->delta_t != 0 && atan_desired_curvature != 0.0)
fprintf(stdout, "d_phi: %.5lf phi: %.5lf\n", simulator_config->target_phi, simulator_config->phi);
return (simulator_config->phi);
}
void
carmen_laser_get_offset(int which, carmen_point_t *laser_offset)
{
char param_name[2048];
char module_name[2048];
int got_module_name = 0;
char *laser_pose_str;
double x, y, theta;
int num_scanned;
int error;
if (carmen_param_get_module() != NULL) {
strcpy(module_name, carmen_param_get_module());
got_module_name = 1;
carmen_param_set_module(NULL);
}
sprintf(param_name, "laser_laser%d_position", which);
error = carmen_param_get_string(param_name, &laser_pose_str, NULL);
if (error == -1)
carmen_die(carmen_param_get_error());
num_scanned = sscanf(laser_pose_str, "%*c %lf %lf %lf %*c", &x, &y,
&theta);
theta = carmen_degrees_to_radians(theta);
if (num_scanned != 3)
carmen_die("Bad string for %s: %s\n", param_name, laser_pose_str);
laser_offset->x = x;
laser_offset->y = y;
laser_offset->theta = theta;
free(laser_pose_str);
if (got_module_name)
carmen_param_set_module(module_name);
}
int
gps_parse_hdt(char *line, int num_chars)
{
char *ptr;
int i;
for (i = 1; i < num_chars-1; i++)
{
if (line[i] == '$' || line[i] == '*')
return(FALSE);
}
if (num_chars > 0 && carmen_extern_gphdt_ptr != NULL)
{
ptr = strsep(&line, ",");
if (ptr == NULL)
return(FALSE);
ptr = strsep(&line, ",");
if (ptr == NULL)
return(FALSE);
if (strlen(ptr) > 0)
{
double heading = atof(ptr);
heading = carmen_degrees_to_radians(-90.0 - heading);
heading = carmen_normalize_theta(heading);
carmen_extern_gphdt_ptr->heading = heading;
carmen_extern_gphdt_ptr->valid = 1;
}
else
{
carmen_extern_gphdt_ptr->heading = 0.0;
carmen_extern_gphdt_ptr->valid = 0;
}
return(TRUE);
}
return(FALSE);
}
