void
carmen_minimize_gridmap(carmen_map_t *map, int *x_offset, int *y_offset)
{
int x, y;
int min_x = map->config.x_size, min_y = map->config.y_size;
int max_x = 0, max_y = 0;
int new_width, new_height;
double **new_map;
double *new_complete_map;
for (x = 0; x < map->config.x_size; x++)
for (y = 0; y < map->config.y_size; y++)
{
if (map->map[x][y] != -1)
{
min_x = carmen_fmin(min_x, x);
min_y = carmen_fmin(min_y, y);
max_x = carmen_fmax(max_x, x);
max_y = carmen_fmax(max_y, y);
}
}
new_width = max_x - min_x + 1;
new_height = max_y - min_y + 1;
new_complete_map = (double *)calloc(new_width*new_height, sizeof(double));
carmen_test_alloc(new_complete_map);
new_map = (double **)calloc(new_width, sizeof(double));
carmen_test_alloc(new_map);
for (x = 0; x < new_width; x++)
{
new_map[x] = new_complete_map+x*new_height;
for (y = 0; y < new_height; y++)
memcpy(new_map[x], map->map[x+min_x]+min_y, new_height*sizeof(double));
}
free(map->map);
map->map = new_map;
free(map->complete_map);
map->complete_map = new_complete_map;
map->config.x_size = new_width;
map->config.y_size = new_height;
*x_offset = min_x;
*y_offset = min_y;
}
double
SpeedControlLogic(carmen_ackerman_path_point_t pose, carmen_simulator_ackerman_config_t *simulator_config)
{
double v_scl = simulator_config->max_v;
//todo transformar esses valores em parametros no ini
double a_scl = 0.1681;
double b_scl = -0.0049;
double safety_factor = 1;
double kt = carmen_get_curvature_from_phi(pose.phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
double v_cmd = fabs(simulator_config->target_v);
double v_cmd_max = carmen_fmax(v_scl, (kt + simulator_config->delta_t - a_scl) / b_scl);
double k_max_scl = carmen_fmin(simulator_config->maximum_steering_command_curvature, a_scl + b_scl * v_cmd);
double kt_dt = carmen_get_curvature_from_phi(simulator_config->target_phi, pose.v, simulator_config->understeer_coeficient, simulator_config->distance_between_front_and_rear_axles);
if (fabs(kt_dt) >= k_max_scl)
v_cmd = fabs(safety_factor * v_cmd_max);
if(pose.v != 0)
return v_cmd * (pose.v / fabs(pose.v));
else
return 0;
}
static int
next_waypoint_astar(carmen_ackerman_traj_point_p waypoint)
{
if (path.path == NULL)
return -1;
// int i;
// carmen_ackerman_traj_point_t pose = *waypoint;
// double min_pose_dist = carmen_distance_ackerman_traj(waypoint, &path.path[0]);
// double pose_dist = 0;
// double temp;
// for (i = 1; i <= 30; i++)
// {
// pose_dist = carmen_distance_ackerman_traj(&pose, &path.path[0]);
// if (pose_dist < 0.1 || pose_dist > min_pose_dist)
// {
// path.path[0].v = (path.path[0].v / fabs(path.path[0].v)) * (i / 20.0);
// break;
// }
// printf("pose_dist %f x %f y %f\n",pose_dist, pose.x, pose.y);
// min_pose_dist = pose_dist;
// pose = predict_new_robot_position(*waypoint, 1, path.path[0].phi, (i / 20.0), &carmen_robot_ackerman_config);
// }
// printf("v %f d_p %f i %d %f\n",path.path[0].v, carmen_distance_ackerman_traj(waypoint, &path.path[0]), i, temp);
double delta_theta = fabs(carmen_normalize_theta(waypoint->theta - path.path[0].theta));
replan = 0;
if (carmen_distance_ackerman_traj(waypoint, &path.path[0]) <= 0.3 && delta_theta < carmen_degrees_to_radians(1.5))
{
delete_path_point(0);
min_delta_d = INTMAX_MAX;
if (path.path_size <= 1)
{
replan = 1;
return 1;
}
}
if (min_delta_d != -1 && carmen_distance_ackerman_traj(waypoint, &path.path[0]) - min_delta_d > 0.5)
{
replan = 1;
return -1;
}
min_delta_d = carmen_fmin(carmen_distance_ackerman_traj(waypoint, &path.path[0]), min_delta_d);
return 0;
}
unsigned char *carmen_graphics_convert_to_image(carmen_map_p map, int flags)
{
register float *data_ptr;
unsigned char *image_data = NULL;
register unsigned char *image_ptr = NULL;
double value;
int x_size, y_size;
int x_index, y_index;
int index;
double max_val = -MAXDOUBLE, min_val = MAXDOUBLE;
int rescale = flags & CARMEN_GRAPHICS_RESCALE;
int invert = flags & CARMEN_GRAPHICS_INVERT;
int rotate = flags & CARMEN_GRAPHICS_ROTATE;
int black_and_white = flags & CARMEN_GRAPHICS_BLACK_AND_WHITE;
if (map == NULL) {
carmen_warn("carmen_graphics_convert_to_image was passed NULL map.\n");
return NULL;
}
x_size = map->config.x_size;
y_size = map->config.y_size;
image_data = (unsigned char *)calloc(x_size*y_size*3, sizeof(unsigned char));
carmen_test_alloc(image_data);
if (rescale) {
max_val = -MAXDOUBLE;
min_val = MAXDOUBLE;
data_ptr = map->complete_map;
for (index = 0; index < map->config.x_size*map->config.y_size; index++) {
max_val = carmen_fmax(max_val, *data_ptr);
if (*data_ptr >= 0)
min_val = carmen_fmin(min_val, *data_ptr);
data_ptr++;
}
}
if (max_val < 0)
rescale = 0;
image_ptr = image_data;
data_ptr = map->complete_map;
for (x_index = 0; x_index < x_size; x_index++) {
for (y_index = 0; y_index < y_size; y_index++) {
value = *(data_ptr++);
if (rotate)
image_ptr = image_data+y_index*x_size*3+x_index;
if (value < 0 && value > -1.5) {
if (black_and_white) {
*(image_ptr++) = 255;
*(image_ptr++) = 255;
*(image_ptr++) = 255;
} else {
*(image_ptr++) = 0;
*(image_ptr++) = 0;
*(image_ptr++) = 255;
}
}
else if (value < -1.5) { // for offlimits
if (black_and_white) {
*(image_ptr++) = 205;
*(image_ptr++) = 205;
*(image_ptr++) = 205;
} else {
*(image_ptr++) = 255;
*(image_ptr++) = 0;
*(image_ptr++) = 0;
}
}
else if(!rescale && value > 1.0) {
if (black_and_white) {
*(image_ptr++) = 128;
*(image_ptr++) = 128;
*(image_ptr++) = 128;
} else {
*(image_ptr++) = 255;
*(image_ptr++) = 0;
*(image_ptr++) = 0;
}
} else {
if (rescale)
value = (value - min_val) / (max_val - min_val);
if (!invert)
value = 1 - value;
for (index = 0; index < 3; index++)
*(image_ptr++) = value * 255;
}
}
}
return image_data;
}
