static void
behaviour_selector_goal_list_message_handler(carmen_behavior_selector_goal_list_message *msg)
{
Pose goal_pose;
if ((msg->size <= 0) || !msg->goal_list || !GlobalState::localize_pose)
return;
GlobalState::last_goal = (msg->size == 1)? true: false;
goal_pose.x = msg->goal_list->x;
goal_pose.y = msg->goal_list->y;
goal_pose.theta = carmen_normalize_theta(msg->goal_list->theta);
GlobalState::robot_config.max_vel = fmin(msg->goal_list->v, GlobalState::param_max_vel);
set_rrt_current_goal(goal_pose);
}
carmen_pose_3D_t
carmen_ackerman_interpolated_robot_position_at_time(carmen_pose_3D_t robot_pose, double dt, double v, double phi, double distance_between_front_and_rear_axles)
{
carmen_pose_3D_t pose = robot_pose;
int i;
int steps = 1;
double ds;
ds = v * (dt / (double) steps);
for (i = 0; i < steps; i++)
{
pose.position.x = pose.position.x + ds * cos(pose.orientation.yaw);
pose.position.y = pose.position.y + ds * sin(pose.orientation.yaw);
pose.orientation.yaw = carmen_normalize_theta(pose.orientation.yaw + ds * (tan(phi) / distance_between_front_and_rear_axles));
}
return pose;
}
carmen_point_t
carmen_point_with_move( carmen_point_t start, carmen_move_t move )
{
carmen_point_t end;
if ( (move.forward==0.0) && (move.sideward==0.0) && (move.rotation==0.0) )
return (start);
end.x =
start.x +
cos(start.theta) * move.forward +
sin(start.theta) * move.sideward;
end.y =
start.y +
sin(start.theta) * move.forward -
cos(start.theta) * move.sideward;
end.theta =
carmen_normalize_theta( start.theta + move.rotation );
return(end);
}
/* initialize particles from a gaussian distribution */
void
carmen_localize_ackerman_initialize_particles_gaussians(carmen_localize_ackerman_particle_filter_p filter,
int num_modes,
carmen_point_t *mean,
carmen_point_t *std)
{
int i, j, each, start, end;
double x, y, theta;
each = (int) floor(filter->param->num_particles / (double) num_modes);
for(i = 0; i < num_modes; i++)
{
start = i * each;
if(i == num_modes - 1)
end = filter->param->num_particles;
else
end = (i + 1) * each;
for(j = start; j < end; j++)
{
x = carmen_gaussian_random(mean[i].x, std[i].x);
y = carmen_gaussian_random(mean[i].y, std[i].y);
theta = carmen_normalize_theta(carmen_gaussian_random(mean[i].theta, std[i].theta));
filter->particles[j].x = x;
filter->particles[j].y = y;
filter->particles[j].theta = theta;
filter->particles[j].weight = 0.0;
}
}
filter->initialized = 1;
filter->first_odometry = 1;
// if (num_modes < 2)
if (0) // @@@ Alberto: mudancca de significado de global_mode para que indique apenas que nao convergiu
filter->global_mode = 0;
else
filter->global_mode = 1;
filter->distance_travelled = 0;
}
Robot_State
TrajectoryLookupTable::predict_next_pose(Robot_State &robot_state, Command &requested_command,
double full_time_interval, double *distance_traveled, double delta_t)
{
int n = floor(full_time_interval / delta_t);
double remaining_time = full_time_interval - ((double) n * delta_t);
Robot_State achieved_robot_state = robot_state; // achieved_robot_state eh computado iterativamente abaixo a partir do estado atual do robo
double curvature = carmen_get_curvature_from_phi(
requested_command.phi, requested_command.v,
GlobalState::robot_config.understeer_coeficient,
GlobalState::robot_config.distance_between_front_and_rear_axles);
double new_curvature = carmen_get_curvature_from_phi(
achieved_robot_state.v_and_phi.phi, achieved_robot_state.v_and_phi.v,
GlobalState::robot_config.understeer_coeficient,
GlobalState::robot_config.distance_between_front_and_rear_axles);
double max_curvature_change = GlobalState::robot_config.maximum_steering_command_rate * delta_t;
// Euler method
for (int i = 0; i < n; i++)
{
double dist_walked = predict_next_pose_step(&achieved_robot_state, &requested_command, delta_t,
new_curvature, curvature, max_curvature_change);
if (distance_traveled)
*distance_traveled += dist_walked;
}
if (remaining_time > 0.0)
{
double dist_walked = predict_next_pose_step(&achieved_robot_state, &requested_command, delta_t,
new_curvature, curvature, max_curvature_change);
if (distance_traveled)
*distance_traveled += dist_walked;
}
achieved_robot_state.pose.theta = carmen_normalize_theta(achieved_robot_state.pose.theta);
robot_state = achieved_robot_state;
return (achieved_robot_state);
}
carmen_point_t
carmen_point_backwards_with_move( carmen_point_t start, carmen_move_t move )
{
carmen_point_t end;
if ( (move.forward==0.0) && (move.sideward==0.0) && (move.rotation==0.0) ) {
return (start);
} else {
end.theta =
carmen_normalize_theta( start.theta - move.rotation );
end.x =
start.x -
cos(end.theta) * move.forward -
sin(end.theta) * move.sideward;
end.y =
start.y -
sin(end.theta) * move.forward +
cos(end.theta) * move.sideward;
}
return(end);
}
carmen_ackerman_traj_point_t
carmen_libcarmodel_recalc_pos_ackerman(carmen_ackerman_traj_point_t robot_state, double target_v, double target_phi,
double full_time_interval, double *distance_traveled, double delta_t, carmen_robot_ackerman_config_t robot_config)
{
double a = (target_v - robot_state.v) / full_time_interval;
int n = floor(full_time_interval / delta_t);
double remaining_time = full_time_interval - ((double) n * delta_t);
carmen_ackerman_traj_point_t achieved_robot_state = robot_state; // achieved_robot_state eh computado iterativamente abaixo a partir do estado atual do robo
double curvature = carmen_get_curvature_from_phi(target_phi, target_v,
robot_config.understeer_coeficient, robot_config.distance_between_front_and_rear_axles);
double new_curvature = carmen_get_curvature_from_phi(achieved_robot_state.phi, achieved_robot_state.v,
robot_config.understeer_coeficient, robot_config.distance_between_front_and_rear_axles);
double max_curvature_change = robot_config.maximum_steering_command_rate * delta_t;
// Euler method
for (int i = 0; i < n; i++)
{
double dist_walked = predict_next_pose_step(&achieved_robot_state, target_v, a, delta_t,
new_curvature, curvature, max_curvature_change, robot_config);
if (distance_traveled)
*distance_traveled += dist_walked;
}
if (remaining_time > 0.0)
{
double dist_walked = predict_next_pose_step(&achieved_robot_state, target_v, a, delta_t,
new_curvature, curvature, max_curvature_change, robot_config);
if (distance_traveled)
*distance_traveled += dist_walked;
}
achieved_robot_state.theta = carmen_normalize_theta(achieved_robot_state.theta);
robot_state = achieved_robot_state;
return (achieved_robot_state);
}
//
// ReadLog
//
// Reads back into the sensor data structures the raw readings that were stored to file by WriteLog (above)
// Reads a single line from the file, and interprets it by its delineator (either Laser or Odometry). If the line
// is not delineated for some reason, the function prints out an error message, and advances to the next line.
// While there is still information in the file, it will return 0. When it reaches the end of the file, it will return 1.
//
int ReadLog(carmen_FILE *logFile, carmen_logfile_index_p logfile_index,
TSense &sense) {
int i, max;
char line[4096];
int laser;
carmen_robot_laser_message laser_msg;
if (carmen_logfile_eof(logfile_index)) {
fprintf(stderr, "End of Log File.\n");
return 1;
}
do {
laser = carmen_logfile_read_next_line(logfile_index, logFile,
4095, line);
} while (strncmp(line, "ROBOTLASER1 ", 12) != 0);
memset(&laser_msg, 0, sizeof(laser_msg));
carmen_string_to_robot_laser_message(line, &laser_msg);
max = laser_msg.num_readings;
if (max > SENSE_NUMBER)
max = SENSE_NUMBER;
// Now read in the whole list of laser readings.
for (i = 0; i < max; i++) {
sense[i].theta = (i*M_PI/max) - M_PI/2;
sense[i].distance = laser_msg.range[i] * MAP_SCALE;
if (sense[i].distance > MAX_SENSE_RANGE)
sense[i].distance = MAX_SENSE_RANGE;
}
// Read x and y coordinates.
odometry.x = laser_msg.laser_pose.x;
odometry.y = laser_msg.laser_pose.y;
// Read the facing angle of the robot.
odometry.theta = laser_msg.laser_pose.theta;
odometry.theta = carmen_normalize_theta(odometry.theta);
return 0;
}
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);
}
/*calculates a laser message based upon the current position*/
void
carmen_simulator_ackerman_calc_laser_msg(carmen_laser_laser_message *laser,
carmen_simulator_ackerman_config_p simulator_config,
int is_rear)
{
carmen_traj_point_t point;
carmen_simulator_ackerman_laser_config_t *laser_config = NULL;
if (is_rear)
{
laser_config = &(simulator_config->rear_laser_config);
}
else
{
laser_config = &(simulator_config->front_laser_config);
}
laser->id = laser_config->id;
point.x = simulator_config->true_pose.x +
laser_config->offset * cos(simulator_config->true_pose.theta) -
laser_config->side_offset * sin(simulator_config->true_pose.theta);
point.y = simulator_config->true_pose.y +
laser_config->offset * sin(simulator_config->true_pose.theta) +
laser_config->side_offset * cos(simulator_config->true_pose.theta);
point.theta = carmen_normalize_theta(simulator_config->true_pose.theta + laser_config->angular_offset);
point.t_vel = simulator_config->v;
point.r_vel = simulator_config->phi;
laser->num_readings = laser_config->num_lasers;
laser->config.maximum_range = laser_config->max_range;
laser->config.fov = laser_config->fov;
laser->config.start_angle = laser_config->start_angle;
laser->config.angular_resolution = laser_config->angular_resolution;
laser->config.laser_type = SIMULATED_LASER;
laser->config.accuracy = laser_config->variance;
//this was placed here because compiling with the old motion model
//did't work, check this if this breaks something
laser->config.remission_mode = REMISSION_NONE;
{
carmen_map_t _map;
carmen_traj_point_t _point;
_map = simulator_config->map;
_map.config.x_origin = _map.config.y_origin = 0;
_point.x = point.x - simulator_config->map.config.x_origin;
_point.y = point.y - simulator_config->map.config.y_origin;
_point.theta = point.theta;
carmen_geometry_generate_laser_data(laser->range, &_point, laser->config.start_angle,
laser->config.start_angle+laser->config.fov,
laser_config->num_lasers,
&_map);
}
carmen_simulator_ackerman_add_objects_to_laser(laser, simulator_config, is_rear);
add_laser_error(laser, laser_config);
}
// updates the internal state variables
int carmen_base_direct_update_status(double* packet_timestamp) //
{
// fill in the time stamp and update internal time
double curr_time = carmen_get_time();
//s_delta_time = curr_time - s_time;
s_time = curr_time;
*packet_timestamp = curr_time;
// sonar is not implemented in orc 5 so we don't deal with this
s_left_range = 0.0;
s_right_range = 0.0;
// ir deleted since not being used
// bumpers
s_bumpers[0] = orc_digital_read( s_orc, ORC_BUMPER_PORT0 );
s_bumpers[1] = orc_digital_read( s_orc, ORC_BUMPER_PORT1 );
s_bumpers[2] = orc_digital_read( s_orc, ORC_BUMPER_PORT2 );
s_bumpers[3] = orc_digital_read( s_orc, ORC_BUMPER_PORT3 );
// read encoders
int curr_left_tick = orc_quadphase_read( s_orc, ORC_LEFT_ENCODER_PORT );
int curr_right_tick = orc_quadphase_read( s_orc, ORC_RIGHT_ENCODER_PORT );
int left_delta = compute_delta_ticks_mod( curr_left_tick, s_left_tick ) * ORC_LEFT_REVERSE_ENCODER ;
int right_delta = compute_delta_ticks_mod( curr_right_tick, s_right_tick ) * ORC_RIGHT_REVERSE_ENCODER ;
s_left_displacement = delta_tick_to_metres( left_delta );
s_right_displacement = delta_tick_to_metres( right_delta );
s_left_tick = curr_left_tick;
s_right_tick = curr_right_tick;
// update velocity information
int left_delta_velocity = orc_quadphase_read_velocity_signed( s_orc, ORC_LEFT_ENCODER_PORT );
int right_delta_velocity = orc_quadphase_read_velocity_signed( s_orc, ORC_RIGHT_ENCODER_PORT );
// looking for underflow or overflow
/*
if( left_delta_velocity > SHORT_MAX/2 )
left_delta_velocity = SHORT_MAX - left_delta_velocity;
if( right_delta_velocity > SHORT_MAX/2 )
right_delta_velocity = SHORT_MAX - right_delta_velocity;
if( left_delta_velocity < -1.0 * SHORT_MAX/2 )
left_delta_velocity = SHORT_MAX + left_delta_velocity;
if( right_delta_velocity < -1.0 * SHORT_MAX/2 )
right_delta_velocity = SHORT_MAX + right_delta_velocity;
*/
s_left_velocity = -1.0 * delta_tick_to_metres( left_delta_velocity ) * 60.0 ; //s_left_displacement / s_delta_time;
s_right_velocity = delta_tick_to_metres( right_delta_velocity ) * 60.0 ; //s_right_displacement / s_delta_time;
//printf( " *** ticks: left %d, right %d \n", left_delta, right_delta );
//printf( " *** ticks: left %d, right %d \n", left_delta_velocity , right_delta_velocity );
//printf( " *** vels: left %f, right %f \n", s_left_velocity, s_right_velocity );
// update position information
double displacement = ( s_left_displacement + s_right_displacement )/2;
double rotation = atan2( s_right_displacement - s_left_displacement,
ORC_WHEEL_BASE );
s_x = s_x + displacement*cos( s_theta );
s_y = s_y + displacement*sin( s_theta );
s_theta = carmen_normalize_theta( s_theta + rotation );
return 0;
}
static carmen_fused_odometry_particle
sample_motion_model(carmen_fused_odometry_particle x_t_1, carmen_fused_odometry_control ut, double dt, carmen_fused_odometry_parameters *fused_odometry_parameters)
{
double L = fused_odometry_parameters->axis_distance;
carmen_fused_odometry_particle x_t = x_t_1;
x_t.state.velocity.x = ut.v +
carmen_gaussian_random(0.0,
fused_odometry_parameters->velocity_noise_velocity * ut.v * ut.v +
fused_odometry_parameters->velocity_noise_phi * ut.phi * ut.phi);
x_t.state.velocity.y = 0.0;
x_t.state.velocity.z = 0.0;
//x_t.state.velocity.z = ut.v_z + carmen_gaussian_random(0.0, alpha5 * ut.v_z * ut.v_z);// + alpha6 * ut.v_pitch * ut.v_pitch);
x_t.state.phi = ut.phi + carmen_gaussian_random(0.0,
fused_odometry_parameters->phi_noise_phi * ut.phi * ut.phi +
fused_odometry_parameters->phi_noise_velocity * ut.v * ut.v);
x_t.state.ang_velocity.roll = 0.0;
// x_t.state.ang_velocity.pitch = 0.0;
// x_t.state.ang_velocity.pitch = ut.v_pitch + carmen_gaussian_random(0.0,
// fused_odometry_parameters->pitch_v_noise_pitch_v * ut.v_pitch * ut.v_pitch +
// fused_odometry_parameters->pitch_v_noise_velocity * ut.v * ut.v);
x_t.state.ang_velocity.yaw = 0.0;
// This will limit the wheel position to the maximum angle the car can turn
if (x_t.state.phi > fused_odometry_parameters->maximum_phi)
{
x_t.state.phi = fused_odometry_parameters->maximum_phi;
}
else if (x_t.state.phi < -fused_odometry_parameters->maximum_phi)
{
x_t.state.phi = -fused_odometry_parameters->maximum_phi;
}
//sensor_vector_imu imu_zt = create_sensor_vector_imu(xsens_matrix_message);
//x_t.state.pose.orientation.roll = imu_zt.orientation.roll;
x_t.state.pose.orientation.roll = ut.roll;
x_t.state.pose.orientation.pitch = ut.pitch;
//x_t.state.pose.orientation.pitch = x_t_1.state.pose.orientation.pitch + x_t.state.ang_velocity.pitch * dt;
//x_t.state.pose.orientation.pitch = 0.0;
x_t.state.pose.orientation.yaw = x_t_1.state.pose.orientation.yaw + (x_t.state.velocity.x * tan(x_t.state.phi) / L) * dt;
x_t.state.pose.orientation.yaw = carmen_normalize_theta(x_t.state.pose.orientation.yaw);
if (0) // (fabs(ut.v) > fused_odometry_parameters->minimum_speed_for_correction) && ut.gps_available)
{
x_t.state.xsens_yaw_bias = x_t_1.state.xsens_yaw_bias * 0.9999 + carmen_gaussian_random(0.0, fused_odometry_parameters->xsens_yaw_bias_noise);
if (x_t.state.xsens_yaw_bias > fused_odometry_parameters->xsens_maximum_yaw_bias)
{
x_t.state.xsens_yaw_bias = fused_odometry_parameters->xsens_maximum_yaw_bias;
}
else if (x_t.state.xsens_yaw_bias < -fused_odometry_parameters->xsens_maximum_yaw_bias)
{
x_t.state.xsens_yaw_bias = -fused_odometry_parameters->xsens_maximum_yaw_bias;
}
}
else
x_t.state.xsens_yaw_bias = 0.0;
tf::Vector3 displacement_car_reference(x_t.state.velocity.x * dt, x_t.state.velocity.y * dt, x_t.state.velocity.z * dt);
tf::Transform car_to_global;
car_to_global.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
car_to_global.setRotation(tf::Quaternion(x_t.state.pose.orientation.yaw, x_t.state.pose.orientation.pitch, x_t.state.pose.orientation.roll));
tf::Vector3 displacement_global_reference = car_to_global * displacement_car_reference;
tf::Vector3 x_t_global_position(x_t_1.state.pose.position.x, x_t_1.state.pose.position.y, x_t_1.state.pose.position.z);
x_t_global_position = x_t_global_position + displacement_global_reference;
double xy_uncertainty_due_to_grid_resolution = (0.2 / 4.0) * (0.2 / 4.0);
double yaw_uncertainty_due_to_grid_resolution = asin((0.2 / 4.0)) * asin((0.2 / 4.0));
x_t.state.pose.position.x = x_t_global_position.getX() + carmen_gaussian_random(0.0, xy_uncertainty_due_to_grid_resolution);
x_t.state.pose.position.y = x_t_global_position.getY() + carmen_gaussian_random(0.0, xy_uncertainty_due_to_grid_resolution);
x_t.state.pose.position.z = 0.0; //x_t_global_position.getZ();
x_t.state.pose.orientation.yaw = carmen_normalize_theta(x_t.state.pose.orientation.yaw + carmen_gaussian_random(0.0, yaw_uncertainty_due_to_grid_resolution));
x_t.weight = x_t_1.weight;
return (x_t);
}
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;
}
particle_datmo_t
sample_motion_model(double delta_time, particle_datmo_t particle_t_1, double mean, particle_datmo_t mean_particle)
{
particle_datmo_t particle_t;
carmen_point_t pose_t, pose_t_1;
double v;
pose_t_1.x = particle_t_1.pose.x;
pose_t_1.y = particle_t_1.pose.y;
pose_t_1.theta = particle_t_1.pose.theta;
v = particle_t_1.velocity;
v = v + carmen_gaussian_random(0.0, alpha_1);
// v = v + carmen_gaussian_random(0.0, mean*0.1);
if (v > v_max) v = v_max;
if (v < v_min) v = v_min;
pose_t_1.theta = pose_t_1.theta + carmen_gaussian_random(0.0, alpha_2);
/* Os resultados melhores estão com inicialização aleatória da orientação utilizando o mesmo desvio padrão fixo.
double c = 100.0;
if (mean > c) {
mean = c;
}
mean = mean * M_PI / c;
double theta_noise = carmen_gaussian_random(0.0, mean);
if(theta_noise > M_PI) {
theta_noise = M_PI;
} else if (theta_noise < -M_PI){
theta_noise = -M_PI;
}
pose_t_1.theta = pose_t_1.theta + theta_noise;
*/
pose_t_1.theta = carmen_normalize_theta(pose_t_1.theta);
pose_t.x = pose_t_1.x + delta_time*v*cos(pose_t_1.theta);
pose_t.y = pose_t_1.y + delta_time*v*sin(pose_t_1.theta);
particle_t.pose.theta = pose_t_1.theta;
particle_t.pose.x = pose_t.x;
particle_t.pose.y = pose_t.y;
particle_t.velocity = v;
particle_t.weight = particle_t_1.weight;
// Keep same class with 90% probability
#ifndef BASELINE
// fixme modificado para o baseline
if ((rand() % 100) <= 90)
//if (carmen_uniform_random(0.0, 100.0) <= 90.0)
particle_t.class_id = particle_t_1.class_id;
else
particle_t.class_id = get_random_model_id(num_of_models);
#else
particle_t.class_id = 0;
#endif
particle_t.model_features = get_obj_model_features(particle_t.class_id);
// particle_t.model_features_3d = get_obj_model_features_3d(particle_t.class_id, object_models_3d);
if(mean > 0 && mean_particle.dist > 0)
{
}
return particle_t;
}
double Util::heading(const Pose &pose, const Pose &target_pose)
{
double target_theta = fabs(carmen_normalize_theta(atan2(pose.y - target_pose.y, pose.x - target_pose.x) - M_PI - pose.theta));
return target_theta / M_PI;
}
void
map_modify_update(carmen_robot_laser_message *laser_msg,
carmen_navigator_config_t *config,
carmen_world_point_p world_point,
carmen_map_p true_map,
carmen_map_p modify_map)
{
int index;
double angle, separation;
int laser_x, laser_y;
double dist;
int increment;
carmen_map_point_t map_point;
int count;
int maxrange_beam;
if (!config->map_update_freespace && !config->map_update_obstacles)
return;
if (true_map == NULL || modify_map == NULL)
{
carmen_warn("%s called with NULL map argument.\n", __FUNCTION__);
return;
}
if (laser_scan == NULL) {
laser_scan = (grid_cell_p *)calloc(LASER_HISTORY_LENGTH, sizeof(grid_cell_p));
carmen_test_alloc(laser_scan);
scan_size = (int *)calloc(LASER_HISTORY_LENGTH, sizeof(int));
carmen_test_alloc(scan_size);
max_scan_size = (int *)calloc(LASER_HISTORY_LENGTH, sizeof(int));
carmen_test_alloc(max_scan_size);
}
update_existing_data(true_map, modify_map);
if (laser_msg->num_readings < config->num_lasers_to_use) {
increment = 1;
separation = laser_msg->config.angular_resolution;
}
else {
increment = laser_msg->num_readings / config->num_lasers_to_use;
if (increment != 1)
separation = carmen_normalize_theta(laser_msg->config.fov / (config->num_lasers_to_use+1));
else
separation = laser_msg->config.angular_resolution;
}
angle = carmen_normalize_theta(world_point->pose.theta +
laser_msg->config.start_angle);
carmen_world_to_map(world_point, &map_point);
count = 0;
for (index = 0; index < laser_msg->num_readings; index += increment) {
if (laser_msg->range[index] < laser_msg->config.maximum_range )
maxrange_beam = 0;
else
maxrange_beam = 1;
if (config->map_update_freespace) {
dist = laser_msg->range[index] - modify_map->config.resolution;
if (dist < 0)
dist = 0;
if (dist > config->map_update_radius)
dist = config->map_update_radius;
laser_x = carmen_round(map_point.x + (cos(angle)*dist)/
modify_map->config.resolution);
laser_y = carmen_round(map_point.y + (sin(angle)*dist)/
modify_map->config.resolution);
trace_laser(map_point.x, map_point.y, laser_x, laser_y, true_map, modify_map);
}
if (config->map_update_obstacles) {
// add obstacle only if it is not a maxrange reading!
if (!maxrange_beam) {
dist = laser_msg->range[index];
laser_x = carmen_round(map_point.x + (cos(angle)*dist)/
modify_map->config.resolution);
laser_y = carmen_round(map_point.y + (sin(angle)*dist)/
modify_map->config.resolution);
if (is_in_map(laser_x, laser_y, modify_map) &&
dist < config->map_update_radius &&
dist < (laser_msg->config.maximum_range - 2*modify_map->config.resolution)) {
count++;
add_filled_point(laser_x, laser_y, true_map, modify_map);
}
}
}
angle += separation;
}
}
int
write_carmen_file( char *filename, logtools_log_data_t *rec )
{
double time, starttime = 0.0;
int i, k, stop;
FILE * iop;
fprintf( stderr, "# write carmen file %s ...\n", filename );
if ((iop = fopen( filename, "w")) == 0){
fprintf(stderr, "# WARNING: can't write carmen file %s\n", filename );
return(-1);
}
stop = FALSE;
for (i=0; i<rec->numentries && !stop; i++) {
switch( rec->entry[i].type ) {
case LASER_VALUES:
starttime = double_time(rec->lsens[rec->entry[i].index].laser.time);
stop = TRUE;
break;
case POSITION:
starttime = double_time(rec->psens[rec->entry[i].index].time);
stop = TRUE;
break;
default:
break;
}
}
fprintf( iop, "# message_name [message contents] ipc_timestamp ipc_hostname logger_timestamp\n" );
fprintf( iop, "# message formats defined: PARAM SYNC ODOM FLASER RLASER TRUEPOS \n" );
fprintf( iop, "# PARAM param_name param_value\n" );
fprintf( iop, "# SYNC tagname\n" );
fprintf( iop, "# ODOM x y theta tv rv accel\n" );
fprintf( iop, "# FLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta\n" );
fprintf( iop, "# RLASER num_readings [range_readings] x y theta odom_x odom_y odom_theta\n" );
fprintf( iop, "# TRUEPOS true_x true_y true_theta odom_x odom_y odom_theta\n");
fprintf( iop, "# NMEA-GGA utc latitude lat_orient longitude long_orient gps_quality num_sattelites hdop sea_level alitude geo_sea_level geo_sep data_age\n");
for (i=0; i<rec->numentries; i++) {
switch( rec->entry[i].type ) {
case LASER_VALUES:
time = double_time( rec->lsens[rec->entry[i].index].laser.time);
switch( rec->lsens[rec->entry[i].index].id ) {
case 0:
fprintf( iop, "FLASER" );
break;
case 1:
fprintf( iop, "RLASER" );
break;
case 2:
fprintf( iop, "LASER2" );
break;
case 3:
fprintf( iop, "LASER3" );
break;
default:
fprintf( iop, "FLASER" );
break;
}
fprintf( iop, " %d", rec->lsens[rec->entry[i].index].laser.numvalues );
for (k=0;k<rec->lsens[rec->entry[i].index].laser.numvalues;k++) {
fprintf(iop, " %.3f", rec->lsens[rec->entry[i].index].laser.val[k]/100.0 );
}
fprintf( iop, " %f %f %f %f %f %f %f %s %f\n",
(float) (rec->lsens[rec->entry[i].index].estpos.x/100.0),
(float) (rec->lsens[rec->entry[i].index].estpos.y/100.0),
(float) (carmen_normalize_theta(rec->lsens[rec->entry[i].index].estpos.o)),
(float) (rec->lsens[rec->entry[i].index].estpos.x/100.0),
(float) (rec->lsens[rec->entry[i].index].estpos.y/100.0),
(float) (carmen_normalize_theta(rec->lsens[rec->entry[i].index].estpos.o)),
time, "nohost", time-starttime );
break;
case POSITION:
time = double_time(rec->psens[rec->entry[i].index].time);
fprintf( iop, "ODOM %f %f %f %f %f %f %f %s %f\n",
(float) (rec->psens[rec->entry[i].index].rpos.x/100.0),
(float) (rec->psens[rec->entry[i].index].rpos.y/100.0),
(float) (carmen_normalize_theta(rec->psens[rec->entry[i].index].rpos.o)),
(float) (rec->psens[rec->entry[i].index].rvel.tv/100.0),
(float) (rec->psens[rec->entry[i].index].rvel.rv/100.0),
0.0, /* acceleration */
time, "nohost", time-starttime );
break;
default:
break;
}
}
return(0);
}
static void
tracker_position_handler(carmen_tracker_position_message *message)
{
static std::vector<double> X;
static std::vector<double> Y;
static std::vector<double> I;
static unsigned int index = 0;
tk::spline x, y;
double distancia;
double angulo;
if (global_localize_ackerman_globalpos_message.size() == 0)
{
printf("globalpos nao inicializado!\n");
return;
}
else
{
for (int i = 0; i < global_localize_ackerman_globalpos_message.size(); i++)
{
if (fabs(message->timestamp - global_localize_ackerman_globalpos_message[i].timestamp) < 0.01)
{
globalpos = global_localize_ackerman_globalpos_message[i].globalpos;
}
}
}
if (globalpos.x == 0 && globalpos.y == 0 && globalpos.theta == 0)
{
printf("globalpos nao inicializado!\n");
return;
}
if (message->object_position.x == 0.0)
{
printf("pose zerada: message->object_position.x %.2f message->object_position.y %.2f\n",message->object_position.x,message->object_position.y);
return;
}
distancia = sqrt(pow(message->object_position.x, 2) + pow(message->object_position.y, 2));
angulo = atan2(message->object_position.y, message->object_position.x);
distancia -= 4;
X.push_back(globalpos.x + distancia * cos(carmen_normalize_theta(globalpos.theta + carmen_degrees_to_radians(angulo))) - 0);
Y.push_back(globalpos.y + distancia * sin(carmen_normalize_theta(globalpos.theta + carmen_degrees_to_radians(angulo))));
I.push_back(index++);
if (I.size() > MAX_POSITIONS)
{
X.erase(X.begin());
Y.erase(Y.begin());
I.erase(I.begin());
}
if (I.size() > 1)
{
x.set_points(I,X);
y.set_points(I,Y);
for (int i = 0; i < I.size() + EXTRA_POSITIONS; i++)
{
poses[i].x = x(I[0] + i);
poses[i].y = y(I[0] + i);
}
}
carmen_point_t goal;
goal.x = poses[I.size() - 1].x;
goal.y = poses[I.size() - 1].y;
goal.theta = globalpos.theta;
carmen_behavior_selector_clear_goal_list();
carmen_behavior_selector_add_goal(goal);
//publish_spline_goal_list_message(poses + I.size() - 1, 1); //todo colocar o menor possivel, pode ajudar.
// publish_spline_path_message(poses, I.size() + EXTRA_POSITIONS);
printf("pose X %f Y %f I %d theta %f\t", message->object_position.x, message->object_position.y, I[I.size() - 1], globalpos.theta);
printf("I.size(): %d\n",I.size());
}
bool Util::is_behind(const Pose &p1, const Pose &p2)
{
return fabs(carmen_normalize_theta(atan2(p1.y - p2.y, p1.x - p2.x) - M_PI - p1.theta)) > M_PI_2;
}
static void update_evidence_grid(carmen_shape_p contour, int *laser_mask, double resolution, double laser_stdev) {
double xmin, xmax, ymin, ymax;
double dx, dy, dmax;
double *ux, *uy;
double **mask;
int i, j, x, y, x0, x1, y0, y1;
int new_x_size, new_y_size;
int mask_x_size, mask_y_size, mask_ux, mask_uy;
int mask_x0, mask_x1, mask_y0, mask_y1;
int x_offset, y_offset;
double ltheta;
printf("update_evidence_grid()\n");
ux = contour->x;
uy = contour->y;
// find bounds for contour's evidence grid
xmin = xmax = ux[0];
ymin = ymax = uy[0];
for (i = 1; i < contour->num_points; i++) {
if (ux[i] > xmax)
xmax = ux[i];
else if (ux[i] < xmin)
xmin = ux[i];
if (uy[i] > ymax)
ymax = uy[i];
else if (uy[i] < ymin)
ymin = uy[i];
}
// give a buffer for the egrid
xmin -= 2 * EGRID_MASK_STDEV_MULT * laser_stdev;
ymin -= 2 * EGRID_MASK_STDEV_MULT * laser_stdev;
xmax += 2 * EGRID_MASK_STDEV_MULT * laser_stdev;
ymax += 2 * EGRID_MASK_STDEV_MULT * laser_stdev;
new_x_size = floor((xmax-xmin) / resolution);
new_y_size = floor((ymax-ymin) / resolution);
//egrid_x_offset = -xmin;
//egrid_y_offset = -ymin;
if (egrid == NULL) { // new egrid
egrid = new_matrix(egrid_x_size, egrid_y_size);
egrid_counts = new_imatrix(egrid_x_size, egrid_y_size);
egrid_hits = new_imatrix(egrid_x_size, egrid_y_size);
for (i = 0; i < egrid_x_size; i++) {
for (j = 0; j < egrid_y_size; j++) {
egrid_counts[i][j] = egrid_hits[i][j] = 0;
egrid[i][j] = -1.0;
}
}
egrid_x_size = new_x_size;
egrid_y_size = new_y_size;
egrid_x_offset = -xmin;
egrid_y_offset = -ymin;
}
else { // resize
x0 = floor((xmin - egrid_x_offset)/resolution);
x1 = floor((xmax - egrid_x_offset)/resolution) + 1;
y0 = floor((ymin - egrid_y_offset)/resolution);
y1 = floor((ymax - egrid_y_offset)/resolution) + 1;
if (x0 < 0 || x1 > egrid_x_size || y0 < 0 || y1 > egrid_y_size) {
x0 = MAX(-x0, 0);
x1 = MAX(x1, egrid_x_size);
y0 = MAX(-y0, 0);
y1 = MAX(y1, egrid_y_size);
egrid = matrix_resize(egrid, x0, y0, egrid_x_size, egrid_y_size, x0+x1, y0+y1, -1.0);
egrid_counts = imatrix_resize(egrid_counts, x0, y0, egrid_x_size, egrid_y_size, x0+x1, y0+y1, 0);
egrid_hits = imatrix_resize(egrid_hits, x0, y0, egrid_x_size, egrid_y_size, x0+x1, y0+y1, 0);
egrid_x_size = x0 + x1;
egrid_y_size = y0 + y1;
egrid_x_offset -= x0*resolution;
egrid_y_offset -= y0*resolution;
}
}
// trace laser
for (i = 0; i < laser_msg.num_readings; i++) {
ltheta = carmen_normalize_theta(laser->theta + (i-90)*M_PI/180.0);
trace_laser_beam(laser->x, laser->y, ltheta, laser->range[i], laser_mask[i]);
}
/***************************** dbug **********************************
for (i = 0; i < egrid_x_size; i++)
for (j = 0; j < egrid_y_size; j++)
egrid[i][j] = LOW_LOG_PROB;
// create a log prob mask of odd dimensions (so there will be a unique center cell)
mask_x_size = floor(2 * EGRID_MASK_STDEV_MULT * laser_stdev / resolution);
if (mask_x_size % 2 == 0)
mask_x_size++;
mask_y_size = floor(2 * EGRID_MASK_STDEV_MULT * laser_stdev / resolution);
if (mask_y_size % 2 == 0)
mask_y_size++;
mask_ux = floor(mask_x_size / 2.0);
mask_uy = floor(mask_y_size / 2.0);
//printf("mask_size = (%d %d)\n", mask_x_size, mask_y_size);
mask = new_matrix(mask_x_size, mask_y_size);
dmax = mask_x_size*mask_x_size*resolution*resolution/4.0;
for (i = 0; i < mask_x_size; i++) {
dx = (i - mask_ux) * resolution;
for (j = 0; j < mask_y_size; j++) {
dy = (j - mask_uy) * resolution;
if (dx*dx+dy*dy <= dmax) // circular mask
mask[i][j] = -(dx*dx+dy*dy) / (2 * laser_stdev * laser_stdev);
else
mask[i][j] = LOW_LOG_PROB;
}
}
for (i = 0; i < contour->num_points; i++) {
x = floor((ux[i] - xmin) / resolution);
y = floor((uy[i] - ymin) / resolution);
if (x < 0 || x >= egrid_x_size || y < 0 || y >= egrid_y_size) {
carmen_warn("Warning: contour point out of evidence grid bounds in get_evidence_grid()\n");
continue;
}
mask_x0 = MAX(0, x + (mask_ux+1) - mask_x_size);
mask_x1 = MIN(egrid_x_size - 1, x - mask_ux + mask_x_size);
mask_y0 = MAX(0, y + (mask_uy+1) - mask_y_size);
mask_y1 = MIN(egrid_y_size - 1, y - mask_uy + mask_y_size);
x_offset = mask_ux - x;
y_offset = mask_uy - y;
for (x = mask_x0; x < mask_x1; x++)
for (y = mask_y0; y < mask_y1; y++)
if (egrid[x][y] < mask[x+x_offset][y+y_offset])
egrid[x][y] = mask[x+x_offset][y+y_offset];
}
free(mask);
//*egrid_x_offset = -xmin;
//*egrid_y_offset = -ymin;
//*egrid_x_size_ptr = egrid_x_size;
//*egrid_y_size_ptr = egrid_y_size;
***************************** end dbug *****************************/
return egrid;
}