Exemple #1
0
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;
}
Exemple #3
0
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);
}
Exemple #6
0
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);
}
Exemple #8
0
//
// 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;
}
Exemple #9
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);
}
Exemple #11
0
// 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;
}
Exemple #15
0
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;
}
Exemple #16
0
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;
  }
}
Exemple #17
0
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());
}
Exemple #19
0
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;
}
Exemple #20
0
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;
}