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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
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();
}
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;
}
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;
}
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(¤t_pos);
}
else { // truncated string given -> cannot read id
laser->id = -1;
}
laser->config.laser_type = CLF_READ_INT(¤t_pos);
laser->config.start_angle = CLF_READ_DOUBLE(¤t_pos);
laser->config.fov = CLF_READ_DOUBLE(¤t_pos);
laser->config.angular_resolution = CLF_READ_DOUBLE(¤t_pos);
laser->config.maximum_range = CLF_READ_DOUBLE(¤t_pos);
laser->config.accuracy = CLF_READ_DOUBLE(¤t_pos);
laser->config.remission_mode = CLF_READ_INT(¤t_pos);
num_readings = CLF_READ_INT(¤t_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(¤t_pos);
num_remissions = CLF_READ_INT(¤t_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(¤t_pos);
laser->timestamp = CLF_READ_DOUBLE(¤t_pos);
copy_host_string(&laser->host, ¤t_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;
}
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));
}
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;
}
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));
}
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;
}
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;
}
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;
}
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;
}
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);
}
}
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;
}
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;
}
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;
}
}
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;
}
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;
}
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;
}