MessageInterpolation<M1, M2>::MessageInterpolation(int list_size) {
	this->interpolation_list = (M1* ) calloc (list_size, sizeof(M1));
	carmen_test_alloc(this->interpolation_list);

	this->interpolation_list_size = list_size;
	this->interpolation_list_counter = 0;
}
示例#2
0
char *
carmen_param_pair_and_remove(const char *lvalue)
{
  int i, j;
  char *value;

  for(i = param_num - 1; i >= 0; i--)
    if(strcmp(param_list[i].lvalue, lvalue) == 0) {
      if(strcmp(param_list[i].robot, "commandline") == 0 &&
	 strlen(param_list[i].rvalue) == 0)
	return NULL;
      else
	{
	  value = (char *)calloc(strlen(param_list[i].rvalue)+1, sizeof(char));
	  carmen_test_alloc(value);

	  strcpy(value, param_list[i].rvalue);

	  for (j = i; j < param_num-1; j++)
	    { 
	      strcpy(param_list[j].robot, param_list[j+1].robot);
	      strcpy(param_list[j].lvalue, param_list[j+1].lvalue);
	      strcpy(param_list[j].rvalue, param_list[j+1].rvalue);
	    }
	  param_num--;

	  return value;
	}
    }
  return NULL;
}
示例#3
0
int main(int argc, char *argv[]) 
{
  unsigned int seed;
  
  carmen_initialize_ipc(argv[0]);

  carmen_param_check_version(argv[0]);
  IPC_setVerbosity(IPC_Print_Warnings);  
  
  seed = carmen_randomize(&argc, &argv);
  carmen_warn("Seed: %u\n", seed);
  carmen_simulator_subscribe_truepos_message
    (&truepose, NULL, CARMEN_SUBSCRIBE_LATEST);

  carmen_simulator_subscribe_objects_message
    (NULL, (carmen_handler_t)simulator_objects_handler, 
     CARMEN_SUBSCRIBE_LATEST);

  map = (carmen_map_t *)calloc(1, sizeof(carmen_map_t));
  carmen_test_alloc(map);
  carmen_map_get_gridmap(map);

  IPC_dispatch();

  return 0;
}
示例#4
0
static void 
add_clear_point(int x, int y, carmen_map_p true_map, carmen_map_p modify_map) 
{
  int num_points = scan_size[current_data_set];
  grid_cell_p cell_list;
  double value;

  if (!is_in_map(x, y, modify_map))
    return;

  value = true_map->map[x][y];

  /*   if (is_empty(value))  */
  /* 		return; */
		
  if (point_exists(x, y, EMPTY))
    return;

  if (num_points == max_scan_size[current_data_set]) {
    max_scan_size[current_data_set] *= 2;
    laser_scan[current_data_set] = 
      realloc(laser_scan[current_data_set], 
	      max_scan_size[current_data_set]*sizeof(grid_cell_t));
    carmen_test_alloc(laser_scan[current_data_set]);
  }

  cell_list = laser_scan[current_data_set];
  value = EMPTY;
  cell_list[num_points].x = x;
  cell_list[num_points].y = y;
  cell_list[num_points].value = value;
  scan_size[current_data_set]++;
  modify_map->map[x][y] = value;
}
示例#5
0
carmen_vector_3D_p
reproject_single_point_to_3D(const stereo_util *camera, carmen_position_t right_point, double disparity)
{
	carmen_vector_3D_p p3D = NULL;

	if (disparity > 0)
	{
		double xr = right_point.x - camera->xc;
		double yr = right_point.y - camera->yc;
		double xl = xr + disparity;

		double fx_fy = camera->fx / camera->fy;

		// transform 2D image point to 3D Carmen coordinate frame
		double X = -(camera->baseline * camera->fx) / (xr - xl);
		double Y = (camera->baseline * (xr + xl)) / (2.0 * (xr - xl));
		double Z = -fx_fy * (camera->baseline * yr) / (xr - xl);

		if (!(isnan(X) || isnan(Y) || isnan(Z) ||
				isinf(X) ||	isinf(Y) ||	isinf(Z)))
		{
			p3D = (carmen_vector_3D_p)malloc(sizeof(carmen_vector_3D_t));
			carmen_test_alloc(p3D);

			p3D->x = X;
			p3D->y = Y;
			p3D->z = Z;
		}
	}
	return p3D;
}
示例#6
0
carmen_camera_image_t *carmen_camera_start(int argc, char **argv)
{
  carmen_camera_image_t *image;

  read_parameters(argc, argv);

  carmen_warn("Opening camera\n");

  camera_fd = open(camera_dev, O_RDONLY | O_NOCTTY);
  if (camera_fd < 0)
    carmen_die_syserror("Could not open %s as camera device", camera_dev);

  check_camera_type();
  setup_camera();

  image = (carmen_camera_image_t *)calloc(1, sizeof(carmen_camera_image_t));

  image->width = image_width;
  image->height = image_height;
  image->bytes_per_pixel = 3;

  image->image_size = image->width*image->height*image->bytes_per_pixel;
  image->is_new = 0;
  image->timestamp = 0;
  image->image = (char *)calloc(image->image_size, sizeof(char));
  carmen_test_alloc(image->image);
  memset(image->image, 0, image->image_size*sizeof(char));

  return image;
}
static void
init_velodyne_points(spherical_point_cloud **velodyne_points_out, unsigned char ***intencity, carmen_pose_3D_t **robot_pose_out, carmen_vector_3D_t **robot_velocity_out, double **robot_timestamp_out, double **robot_phi_out)
{
	int i;

	carmen_pose_3D_t *robot_pose = (carmen_pose_3D_t *)calloc(NUM_VELODYNE_POINT_CLOUDS, sizeof(carmen_pose_3D_t));
	carmen_vector_3D_t *robot_velocity = (carmen_vector_3D_t *)calloc(NUM_VELODYNE_POINT_CLOUDS, sizeof(carmen_vector_3D_t));
	spherical_point_cloud *velodyne_points = (spherical_point_cloud *)malloc(NUM_VELODYNE_POINT_CLOUDS * sizeof(spherical_point_cloud));
	double *robot_timestamp = (double *)calloc(NUM_VELODYNE_POINT_CLOUDS, sizeof(double));
	*intencity = (unsigned char **)calloc(NUM_VELODYNE_POINT_CLOUDS, sizeof(unsigned char*));
	*robot_phi_out = (double *)calloc(NUM_VELODYNE_POINT_CLOUDS, sizeof(double));


	carmen_test_alloc(velodyne_points);

	for (i = 0; i < NUM_VELODYNE_POINT_CLOUDS; i++)
	{
		velodyne_points[i].num_points = 0;
		velodyne_points[i].sphere_points = NULL;
	}

	*velodyne_points_out = velodyne_points;
	*robot_pose_out = robot_pose;
	*robot_velocity_out = robot_velocity;
	*robot_timestamp_out = robot_timestamp;
}
示例#8
0
void 
MessageControl::carmen_planner_ackerman_get_status(carmen_planner_status_p status)
{
	int index;
	if (!requested_goal)
		return;

	status->goal = *requested_goal;
	status->robot = robot;
	status->goal_set = (requested_goal == NULL);
	status->path.length = path.length;

	if (status->path.length > 0)
	{
		status->path.points = (carmen_ackerman_traj_point_p) calloc(status->path.length, sizeof(carmen_ackerman_traj_point_t));
		carmen_test_alloc(status->path.points);
		for (index = 0; index < status->path.length; index++)
			status->path.points[index] = path.points[index];
	}
	else
	{
		status->path.length = 0;
	}
	return;
}
示例#9
0
static void image_handler(carmen_camera_image_message *image_msg)
{
  int i, j;
  unsigned char *data;

  if (!received_image) {
    gtk_widget_set_usize (drawing_area, image_msg->width, image_msg->height);
  } else
    g_object_unref(image);

  data = (unsigned char *)calloc
    (image_msg->width*image_msg->height*3, sizeof(unsigned char));
  carmen_test_alloc(data);

  for (i = 0; i < image_msg->width*image_msg->height; i++) {
    for (j = 0; j < 3; j++) {
      data[3*i+j] = image_msg->image[3*i+j];
    }
  }

  image = gdk_pixbuf_new_from_data((guchar *)data, GDK_COLORSPACE_RGB,
				   FALSE, 8,  image_msg->width, 
				   image_msg->height, image_msg->width*3, 
				   pixbuf_destroyed, NULL);

  received_image = 1;
  redraw();
}
示例#10
0
carmen_vector_3D_p
reproject_single_point_to_3D_with_depth(stereo_util instance, carmen_position_t right_point, unsigned short depth)
{
	carmen_vector_3D_p p3D = NULL;
	double z, vx, vy, vz;
	double dfx_inv, dfy_inv;

	z = depth / 1000.0f; // load and convert: mm -> meters
	dfx_inv = 1.0 / instance.fx;
	dfy_inv = 1.0 / instance.fy;

	p3D = (carmen_vector_3D_p)malloc(sizeof(carmen_vector_3D_t));
	carmen_test_alloc(p3D);

	p3D->x = 0.0;
	p3D->y = 0.0;
	p3D->z = 0.0;

	vx = z * (right_point.x - instance.xc) * dfx_inv;
	vy = z * (right_point.y - instance.yc) * dfy_inv;
	vz = z;

	if (!(isnan(vx) || isnan(vy) || isnan(vz) || isinf(vx) || isinf(vy) || isinf(vz)))
	{
		p3D->x = vx;
		p3D->y = vy;
		p3D->z = vz;
	}

	return p3D;
}
示例#11
0
carmen_vector_3D_p
reproject_single_point_to_3D_in_left_camera_frame_with_bias_reduction(carmen_position_t left_camera_projection, carmen_position_t right_camera_projection, double baseline, double focal_length, double bias_std_deviation)
{
	// a funcao abaixo usa o codigo do paper para fazer a projecao 3d
	carmen_vector_3D_p biased_point_3D = reproject_single_point_to_3D_in_left_camera_frame(left_camera_projection, right_camera_projection, baseline, focal_length);

	// a funcao abaixo usa o codigo do carmen para fazer a projecao 3d
	// stereo_util camera = get_stereo_instance(6, 1280, 960);
	// carmen_vector_3D_p biased_point_3D = reproject_single_point_to_3D(&camera, right_camera_projection, left_camera_projection.x - right_camera_projection.x);

	if (biased_point_3D != NULL)
	{
		carmen_vector_3D_p point_3D = (carmen_vector_3D_p) calloc (1, sizeof(carmen_vector_3D_t));
		carmen_test_alloc(point_3D);

		// x = x' - (b * v * (xl + xr)) / (xl - xr)^3
		point_3D->x = biased_point_3D->x -
			(baseline * bias_std_deviation * ((left_camera_projection.x + right_camera_projection.x) /
				pow(left_camera_projection.x - right_camera_projection.x, 3)));

		// y = y' - (2 * v * b * yl) / (xl - xr)^3
		point_3D->y = biased_point_3D->y -
			((2 * bias_std_deviation * baseline * left_camera_projection.y) /
					pow(left_camera_projection.x - right_camera_projection.x ,3));

		// z = z' - (2 * v * b * f) / (xl - xr)^3
		point_3D->z = biased_point_3D->z -
			((2 * bias_std_deviation * baseline * focal_length) /
					pow(left_camera_projection.x - right_camera_projection.x ,3));

		return point_3D;
	}
	else
		return NULL;
}
/* initialize particle positions and weights from parameters */
void
carmen_localize_ackerman_initialize_particles_manual(carmen_localize_ackerman_particle_filter_p filter,
		double *x, double *y, double *theta,
		double *weight, int num_particles)
{
	int i;

	if(num_particles != filter->param->num_particles) {
		filter->particles =
				(carmen_localize_ackerman_particle_p)realloc(filter->particles, num_particles *
						sizeof(carmen_localize_ackerman_particle_t));
		carmen_test_alloc(filter->particles);
		realloc_temp_weights(filter, num_particles);
		filter->param->num_particles = num_particles;
	}
	for(i = 0; i < filter->param->num_particles; i++) {
		filter->particles[i].x = x[i];
		filter->particles[i].y = y[i];
		filter->particles[i].theta = theta[i];
		filter->particles[i].weight = weight[i];
	}
	filter->initialized = 1;
	filter->first_odometry = 1;
	filter->global_mode = 0;
	filter->distance_travelled = 0;
}
示例#13
0
char *carmen_string_to_laser_laser_message(char *string,
					   carmen_laser_laser_message *laser)
{
  char *current_pos = string;
  int i, num_readings, num_remissions;

  if (strncmp(current_pos, "RAWLASER", 8) == 0) {
    current_pos += 8;
    laser->id = CLF_READ_INT(&current_pos);
  } 
  else { // truncated string given -> cannot read id
    laser->id = -1;
  }

  laser->config.laser_type = CLF_READ_INT(&current_pos);
  laser->config.start_angle = CLF_READ_DOUBLE(&current_pos);
  laser->config.fov = CLF_READ_DOUBLE(&current_pos);
  laser->config.angular_resolution = CLF_READ_DOUBLE(&current_pos);
  laser->config.maximum_range = CLF_READ_DOUBLE(&current_pos);
  laser->config.accuracy = CLF_READ_DOUBLE(&current_pos);
  laser->config.remission_mode = CLF_READ_INT(&current_pos);

  num_readings = CLF_READ_INT(&current_pos);
  if(laser->num_readings != num_readings) {
    laser->num_readings = num_readings;
    laser->range = (float *)realloc(laser->range, laser->num_readings * 
				    sizeof(float));
    carmen_test_alloc(laser->range);
  }
  for(i = 0; i < laser->num_readings; i++)
    laser->range[i] = CLF_READ_DOUBLE(&current_pos);

  num_remissions = CLF_READ_INT(&current_pos);
  if(laser->num_remissions != num_remissions) {
    laser->num_remissions = num_remissions;
    laser->remission = (float *)realloc(laser->remission, 
					laser->num_remissions * sizeof(float));
    carmen_test_alloc(laser->remission);
  }
  for(i = 0; i < laser->num_remissions; i++)
    laser->remission[i] = CLF_READ_DOUBLE(&current_pos);

  laser->timestamp = CLF_READ_DOUBLE(&current_pos);
  copy_host_string(&laser->host, &current_pos);

  return current_pos;
}
LandmarkSlamMessagesInterpolation<M1, M2>::LandmarkSlamMessagesInterpolation(int list_size) {
    this->interpolation_list = (M1* ) calloc (list_size, sizeof(M1));
    carmen_test_alloc(this->interpolation_list);

    this->interpolation_list_size = list_size;
    this->interpolation_list_initialized = 0;
    this->interpolation_list_counter = 0;
}
carmen_localize_ackerman_particle_filter_p
carmen_localize_ackerman_particle_filter_new(carmen_localize_ackerman_param_p param)
{
	carmen_localize_ackerman_particle_filter_p filter;

	/* allocate the particle filter */
	carmen_set_random_seed(time(NULL));

	filter =
			(carmen_localize_ackerman_particle_filter_p)
			calloc(1, sizeof(carmen_localize_ackerman_particle_filter_t));
	carmen_test_alloc(filter);

	/* set the parameters */
	filter->param = param;

	/* allocate the initial particle set */
	filter->particles =
			(carmen_localize_ackerman_particle_p)calloc(filter->param->num_particles,
					sizeof(carmen_localize_ackerman_particle_t));
	carmen_test_alloc(filter->particles);

	filter->swarm_pbest =
				(carmen_localize_ackerman_particle_p)calloc(filter->param->num_particles,
						sizeof(carmen_localize_ackerman_particle_t));
		carmen_test_alloc(filter->swarm_pbest);

	filter->swarm_velocity =
			(carmen_localize_ackerman_particle_p)calloc(filter->param->num_particles,
					sizeof(carmen_localize_ackerman_particle_t));
	carmen_test_alloc(filter->swarm_velocity);

	/* initialize the temporary weights */
	initialize_temp_weights(filter);

	/* filter has not been initialized */
	filter->initialized = 0;
	filter->first_odometry = 1;
	filter->global_mode = 0;
	filter->distance_travelled = 0;

	filter->param->laser_skip = 0; /* will be automatically initialized later on */

	return filter;
}
示例#16
0
void IMUMessage::init() {

  if (m_msg != NULL) {
    this->free();
  }
  m_msg = new carmen_imu_message;
  carmen_test_alloc(m_msg);
  carmen_erase_structure(m_msg, sizeof(carmen_imu_message));
}
示例#17
0
int carmen_initialize_joystick(carmen_joystick_type *joystick)
{
  if ((joystick->fd = open(JOYSTICK_DEVICE, O_RDONLY | O_NONBLOCK)) < 0){
    carmen_warn("Warning: could not initialize joystick.\n");
    joystick->initialized = 0;
    return -1;
  }
  ioctl(joystick->fd, JSIOCGAXES, &joystick->nb_axes);
  ioctl(joystick->fd, JSIOCGBUTTONS, &joystick->nb_buttons);

  joystick->axes = (int *)calloc(joystick->nb_axes, sizeof(int));
  carmen_test_alloc(joystick->axes);
  joystick->buttons = (int *)calloc(joystick->nb_buttons, sizeof(int));
  carmen_test_alloc(joystick->buttons);
  joystick->initialized = 1;
  
  return 0;
}
示例#18
0
void TrueposMessage::init() {

  if (m_msg != NULL) {
    this->free();
  }
  m_msg = new carmen_simulator_truepos_message;
  carmen_test_alloc(m_msg);
  carmen_erase_structure(m_msg, sizeof(carmen_simulator_truepos_message));
}
示例#19
0
void OdometryMessage::init() {

  if (m_msg != NULL) {
    this->free();
  }
  m_msg = new carmen_base_odometry_message;
  carmen_test_alloc(m_msg);
  carmen_erase_structure(m_msg, sizeof(carmen_base_odometry_message));
}
static inline queue make_queue(void) 
{
  queue new_queue;
  
  new_queue=(queue)calloc(1, sizeof(queue_struct));
  carmen_test_alloc(new_queue);

  return new_queue;
}
示例#21
0
carmen_linemapping_segment_set_t 
carmen_linemapping_get_segments_from_points(const carmen_linemapping_dyn_tab_point_t *pnts) {

  carmen_linemapping_dyn_tab_segment_t *segments_temp = new carmen_linemapping_dyn_tab_segment_t(100);
  carmen_test_alloc(segments_temp);
  carmen_linemapping_split_and_merge(segments_temp, pnts, 0, pnts->numberOfElements()-1);
  
  // just copy the elements of 'segments_temp' into 'segments'
  carmen_linemapping_segment_set_t segments;
  segments.num_segs = segments_temp->numberOfElements();
  segments.segs = new carmen_linemapping_segment_t[segments.num_segs];
  carmen_test_alloc(segments.segs);
  carmen_linemapping_segment_t** elements = segments_temp->getElements();
  for(int i=0; i<segments.num_segs; i++){ segments.segs[i] = *(elements[i]); }

  segments_temp->setAutoDelete(true);
  delete segments_temp;
  return segments;
}
示例#22
0
void LaserMessage::initRemission(int num_remissions) {
  freeRemission();
  m_msg->num_remissions = num_remissions;
  if (num_remissions>0) {
    m_msg->remission = new double[num_remissions];
    carmen_test_alloc(m_msg->remission);
  }
  else 
    m_msg->remission = NULL;
}
示例#23
0
void LaserMessage::initRange(int num_readings) {
  freeRange();
  m_msg->num_readings = num_readings;
  if (num_readings>0) {
    m_msg->range = new double[num_readings];
    carmen_test_alloc(m_msg->range);
  }
  else
    m_msg->range = NULL;
}
示例#24
0
static void
check_param_space()
{
    if (param_list == NULL)
    {
        param_table_capacity = 30;
        param_list = (carmen_ini_param_p)
                     calloc(param_table_capacity, sizeof(carmen_ini_param_t));
        carmen_test_alloc(param_list);
    }

    if (num_params == param_table_capacity)
    {
        param_table_capacity *= 2;
        param_list =
            realloc(param_list, param_table_capacity*sizeof(carmen_ini_param_t));
        carmen_test_alloc(param_list);
    }
}
示例#25
0
glv_object_p glv_object_init(void) {
  glv_object_p obj;

  obj = (glv_object_p)calloc(1, sizeof(glv_object_t));
  carmen_test_alloc(obj);
  obj->num_points = 0;
  obj->max_points = 100000;
  obj->point = (glv_point_p)calloc(obj->max_points, sizeof(glv_point_t));
  carmen_test_alloc(obj->point);
  obj->num_lines = 0;
  obj->max_lines = 100000;
  obj->line = (glv_line_p)calloc(obj->max_lines, sizeof(glv_line_t));
  carmen_test_alloc(obj->line);
  obj->num_faces = 0;
  obj->max_faces = 100000;
  obj->face = (glv_face_p)calloc(obj->max_faces, sizeof(glv_face_t));
  carmen_test_alloc(obj->face);
  return obj;
}
示例#26
0
void
vascocore_alloc_history( carmen_vascocore_history_t * history )
{
  int i, j, nh;

  nh = carmen_vascocore_settings.local_map_history_length;
  nh = (nh>0?nh:1);

  history->data =
    (carmen_vascocore_extd_laser_t *)
    malloc(nh * sizeof(carmen_vascocore_extd_laser_t));
  carmen_test_alloc(history->data);

  for (i=0; i<nh; i++) {
    history->data[i].estpos.x     = 0;
    history->data[i].estpos.y     = 0;
    history->data[i].estpos.theta = 0;
    history->data[i].time         = 0;
    history->data[i].bbox.min.x   = 0;
    history->data[i].bbox.min.y   = 0;
    history->data[i].bbox.max.x   = 0;
    history->data[i].bbox.max.y   = 0;
    history->data[i].numvalues    = 0;
    history->data[i].val        =
      (double *) malloc( MAX_NUM_LASER_VALUES * sizeof(double) );
    carmen_test_alloc(history->data[i].val);
    history->data[i].angle      =
      (double *) malloc( MAX_NUM_LASER_VALUES * sizeof(double) );
    carmen_test_alloc(history->data[i].angle);
    history->data[i].coord      =
      (carmen_vec2_t *) malloc( MAX_NUM_LASER_VALUES * sizeof(carmen_vec2_t) );
    carmen_test_alloc(history->data[i].coord);
    for (j=0; j<MAX_NUM_LASER_VALUES; j++) {
      history->data[i].coord[j].x  = 0;
      history->data[i].coord[j].y  = 0;
      history->data[i].angle[j]    = 0;
      history->data[i].val[j]      = 0;
    }
  }
  history->length = nh;
  history->ptr    = 0;
}
示例#27
0
static void check_message_data_chunk_sizes(void)
{
  int first = 1;

  if (first) {
    robot_bumper.num_bumpers = num_bumpers;
    robot_bumper.state = (char *)calloc(num_bumpers, sizeof(char));
    carmen_test_alloc(robot_bumper.state);
    first = 0;
  } 
}
示例#28
0
HandlerList CreateHandlerList(int number_handlers)
{
  HandlerList handler_list;
  
  handler_list = (HandlerList) calloc((size_t)sizeof(HandlerData),
				      (size_t)number_handlers);
  carmen_test_alloc(handler_list);
  bzero(handler_list,sizeof(HandlerData));
  
  return handler_list;
}
示例#29
0
char *SETUP_ExtGetValue(char *Section, char *Key)
{
  char *tmp, *ret; 

  tmp = calloc(strlen(Section) + strlen(Key) + 3, 1);
  carmen_test_alloc(tmp);
  sprintf(tmp, "[%s]%s", Section, Key);
  ret = SETUP_GetValue(tmp);
  free(tmp);
  return ret;
}
示例#30
0
static int
search_path_add(char *path)
{
  if (sp_num >= SP_SIZE)
    return -1;

  search_path[sp_num] = (char *) calloc(strlen(path) + 1, sizeof(char));
  carmen_test_alloc(search_path[sp_num]);
  strcpy(search_path[sp_num], path);
  sp_num++;
  return 0;
}