int
carmen_parse_arm_joint_types(char *joint_string, carmen_arm_joint_t *joint_types,
int num_joints)
{
char *s;
int n, i;
if (!joint_string || !joint_types)
{
carmen_warn("Bug in carmen_parse_arm_joint_types: a pointer arg was "
"passed as NULL\n");
return -1;
}
s = joint_string;
for (i = 0; i < num_joints; i++) {
s += strspn(s, " \t");
if (strlen(s) == 0) {
carmen_warn("Too few arguments in joint types string.\n");
return -1;
}
n = strcspn(s, " \t");
if (!carmen_strncasecmp(s, "motor", n))
joint_types[i] = CARMEN_MOTOR;
else if (!carmen_strncasecmp(s, "servo", n))
joint_types[i] = CARMEN_SERVO;
else {
carmen_warn("Bad argument (#%d) in joint types string.\n", i+1);
return -1;
}
}
return 0;
}
int
main(int argc, char** argv)
{
double length;
char *dev;
int num_items;
char **modules;
int num_modules;
carmen_param_t param_list[] = {
{"robot", "length", CARMEN_PARAM_DOUBLE, &length, 1, handler},
{"scout", "dev", CARMEN_PARAM_STRING, &dev, 1, handler},
};
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
signal(SIGINT, shutdown_module);
num_items = sizeof(param_list)/sizeof(param_list[0]);
carmen_param_install_params(argc, argv, param_list, num_items);
carmen_param_get_robot();
carmen_param_get_modules(&modules, &num_modules);
carmen_warn("robot_length is %.1f\n", length);
carmen_warn("scout_dev is %s\n", dev);
carmen_param_check_unhandled_commandline_args(argc, argv);
carmen_ipc_dispatch();
return 0;
}
unsigned int
carmen_generate_random_seed(void)
{
FILE *random_fp;
unsigned int seed;
int ints;
random_fp = fopen("/dev/random", "r");
if (random_fp == NULL) {
carmen_warn("Could not open /dev/random for reading: %s\n"
"Using time ^ PID\n", strerror(errno));
seed = time(NULL) ^ getpid();
srandom(seed);
return seed;
}
ints = fread(&seed, sizeof(int), 1, random_fp);
if (ints != 1) {
carmen_warn("Could not read an int from /dev/random: %s\n"
"Using time ^ PID\n", strerror(errno));
seed = time(NULL) ^ getpid();
srandom(seed);
return seed;
}
fclose(random_fp);
srandom(seed);
return seed;
}
void robot_rearlaser_handler(carmen_robot_laser_message *rear_laser)
{
carmen_warn("\nrear_laser\n");
carmen_warn("%.2f %.2f %.2f\n", rear_laser->robot_pose.x,
rear_laser->robot_pose.y,
carmen_radians_to_degrees(rear_laser->robot_pose.theta));
}
void robot_frontlaser_handler(carmen_robot_laser_message *front_laser)
{
carmen_warn("front_laser\n");
carmen_warn("%.2f %.2f %.2f\n", front_laser->robot_pose.x,
front_laser->robot_pose.y,
carmen_radians_to_degrees(front_laser->robot_pose.theta));
}
unsigned int
carmen_randomize(int *argc, char ***argv)
{
long long int user_seed;
unsigned int seed;
int bytes_to_move;
int i;
char *endptr;
for (i = 0; i < *argc-1; i++) {
if (strcmp((*argv)[i], "--seed") == 0) {
user_seed = strtoll((*argv)[i+1], &endptr, 0);
seed = (unsigned int)user_seed;
if (endptr && *endptr != '\0') {
carmen_warn("Bad random seed %s.\n", (*argv)[i+1]);
seed = carmen_generate_random_seed();
} else if (seed != user_seed) {
carmen_warn("Random seed too large: %s.\n", (*argv)[i+1]);
seed = carmen_generate_random_seed();
} else {
if (i < *argc-2) {
bytes_to_move = (*argc-2-i)*sizeof(char *);
memmove((*argv)+i, (*argv)+i+2, bytes_to_move);
}
(*argc) -= 2;
srandom(seed);
}
return seed;
}
}
seed = carmen_generate_random_seed();
return seed;
}
void carmen_velodyne_set_spin_rate(int spin_rate) {
SerialConnection serialConnection(dev_name);
Controller control(serialConnection);
try {
control.setRPM(spin_rate);
}
catch (IOException& exception) {
carmen_warn("\nWarning: Failed to set spin rate\n");
}
catch (OutOfBoundException<size_t>& exception) {
carmen_warn("\nWarning: Spin rate %d is invalid\n", spin_rate);
}
}
void
visual_tracker_test_message_handler(carmen_visual_tracker_test_message* message)
{
IplImage *frame = cvCreateImage(cvSize(message->width, message->height), IPL_DEPTH_8U, message->channels);
copy_RGB_image_to_BGR_image(message->image, frame, message->channels);
if (m_tld && m_tld->initialized())
{
static int fn = 0;
m_tld->process(frame);
message_output.confidence = m_tld->getOutputConfidence();
TLDRect box = m_tld->getOutputRect();
message_output.rect.x = box.x();
message_output.rect.y = box.y();
message_output.rect.width = box.width();
message_output.rect.height = box.height();
carmen_warn("[%d; %.1f; %.1f]\n", ++fn,
box.x()+box.width()/2.0, box.y()+box.height()/2.0);
message_output.host = message->host;
message_output.timestamp = message->timestamp;
publish_visual_tracker_output_message();
}
cvReleaseImage(&frame);
}
void
visual_tracker_train_message_handler(carmen_visual_tracker_train_message* message)
{
IplImage *frame = cvCreateImage(cvSize(message->width, message->height), IPL_DEPTH_8U, message->channels);
copy_RGB_image_to_BGR_image(message->image, frame, message->channels);
TLDRect box(message->rect.x, message->rect.y, message->rect.width, message->rect.height);
if (m_tld)
{
free(m_tld);
m_tld = NULL;
}
m_tld = new Detector();
if (m_tld)
{
m_tld->init(frame, &box);
m_tld->signalBoundingBox(true);
m_tld->signalConfidence(true);
carmen_warn("[%d; %.1f; %.1f]\n", 0,
box.x()+box.width()/2.0, box.y()+box.height()/2.0);
}
cvReleaseImage(&frame);
}
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;
}
void carmen_robot_correct_bumper_and_publish(void)
{
double bumper_skew;
double fraction;
int low, high;
if(!bumper_ready)
return;
check_message_data_chunk_sizes();
bumper_ready = carmen_robot_get_skew(bumper_count, &bumper_skew,
&bumper_average,
base_bumper.host);
if (!bumper_ready) {
carmen_warn("Waiting for bumper data to accumulate\n");
return;
}
fraction = carmen_robot_get_fraction(base_bumper.timestamp, bumper_skew,
&low, &high);
if (!carmen_robot_config.interpolate_odometry)
fraction=0;
construct_bumper_message(&robot_bumper, low, high, fraction);
IPC_RETURN_TYPE err;
err = IPC_publishData(CARMEN_ROBOT_BUMPER_NAME, &robot_bumper);
carmen_test_ipc_exit(err, "Could not publish", CARMEN_ROBOT_BUMPER_NAME);
fprintf(stderr, "b");
bumper_ready = 0;
}
void bumblebee_message_handler(carmen_bumblebee_basic_stereoimage_message *message)
{
static int fn = 1;
char name[255];
CvRect rect_translate = cvRect(160, 90, 10, 12);
CvRect rect_rotate = cvRect(177, 84, 10, 12);
CvRect rect_scale = cvRect(162, 94, 10, 12);
// CvRect *rect = NULL;
CvRect *rect = &rect_translate;
sprintf(name, "camshift-translate-%05d.jpg", fn++);
// CvRect *rect = &rect_rotate;
// sprintf(name, "camshift-rotate-%05d.jpg", fn++);
// CvRect *rect = &rect_scale;
// sprintf(name, "camshift-scale-%05d.jpg", fn++);
CvSize size = cvSize(message->width, message->height);
IplImage *frame_left = cvCreateImage(size, IPL_DEPTH_8U, 3);
copy_RGB_image_to_BGR_image(message->raw_left, frame_left, 3);
cvSaveImage(name, frame_left, 0);
if (camshift)
camshift->trackVisualControl(frame_left);
else
{
camshift = new VisualControl(frame_left->width, frame_left->height);
camshift->startVisualControl(frame_left, rect);
}
carmen_warn("%.1f,%.1f,%.1f,%.1f\n",
camshift->getRectCoordLeftUp().x,
camshift->getRectCoordLeftUp().y,
camshift->getRectCoordRightDown().x,
camshift->getRectCoordRightDown().y);
}
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 toppm(int argc, char *argv[])
{
int err;
int next_arg;
int force;
char *input_filename, *output_filename;
carmen_map_t map;
next_arg = handle_options(argc, argv, &force);
if(argc - next_arg != 3) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s toppm <map filename> <ppm filename>\n\n",
argv[0]);
}
input_filename = check_mapfile(argv[next_arg+1]);
output_filename = check_output(argv[next_arg+2], force);
if(carmen_map_read_gridmap_chunk(input_filename, &map) < 0)
carmen_die("Error: %s did not contain a gridmap.\n", input_filename);
carmen_map_read_offlimits_chunk_into_map(input_filename, &map);
err = carmen_map_write_to_ppm(&map, output_filename);
if (err < 0)
carmen_die_syserror("Could not write ppm to %s", output_filename);
}
int main(int argc, char **argv)
{
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
signal(SIGINT, shutdown_module);
#ifndef COMPILE_WITHOUT_LASER_SUPPORT
carmen_robot_subscribe_frontlaser_message
(NULL, (carmen_handler_t)robot_frontlaser_handler,
CARMEN_SUBSCRIBE_LATEST);
carmen_robot_subscribe_rearlaser_message
(NULL,(carmen_handler_t)robot_rearlaser_handler,
CARMEN_SUBSCRIBE_LATEST);
#endif
carmen_robot_subscribe_sonar_message
(NULL, (carmen_handler_t)robot_sonar_handler,
CARMEN_SUBSCRIBE_LATEST);
carmen_base_subscribe_odometry_message
(NULL, (carmen_handler_t)base_odometry_handler,
CARMEN_SUBSCRIBE_LATEST);
carmen_robot_move_along_vector(0, M_PI/2);
sleep(10);
carmen_robot_move_along_vector(2, 0);
while(1) {
carmen_ipc_sleep(0.1);
carmen_warn(".");
}
return 0;
}
int carmen_velodyne_read_parameters(int argc, char **argv) {
char module[] = "velodyne";
int num_params;
char dev_name_var [256], spin_rate_var[256];
char calib_filename_var[256], dump_enabled_var[256], dump_dirname_var[256];
char points_publish_var[256];
if (argc == 2)
id = atoi(argv[1]);
sprintf(dev_name_var, "velodyne%d_dev", id);
sprintf(spin_rate_var, "velodyne%d_spin_rate", id);
sprintf(calib_filename_var, "velodyne%d_calib_file", id);
sprintf(dump_enabled_var, "velodyne%d_dump_enable", id);
sprintf(dump_dirname_var, "velodyne%d_dump_dir", id);
sprintf(points_publish_var, "velodyne%d_points_publish", id);
carmen_param_t params[] = {
{module, dev_name_var, CARMEN_PARAM_STRING, &dev_name, 1, 0},
{module, spin_rate_var, CARMEN_PARAM_INT, &spin_rate, 1,
carmen_velodyne_spin_rate_handler},
{module, calib_filename_var, CARMEN_PARAM_FILE, &calib_filename, 1, 0},
{module, dump_enabled_var, CARMEN_PARAM_ONOFF, &dump_enabled, 1,
carmen_velodyne_dump_handler},
{module, dump_dirname_var, CARMEN_PARAM_DIR, &dump_dirname, 1,
carmen_velodyne_dump_handler},
{module, points_publish_var, CARMEN_PARAM_ONOFF, &points_publish, 1,
carmen_velodyne_points_handler},
};
num_params = sizeof(params)/sizeof(carmen_param_t);
carmen_param_install_params(argc, argv, params, num_params);
if (!realpath(calib_filename, calib_path)) {
calib_path[0] = '\0';
carmen_warn("\nWarning: Calibration filename %s is invalid\n",
calib_filename);
}
if (!realpath(dump_dirname, dump_path)) {
dump_path[0] = '\0';
carmen_warn("\nWarning: Dump directory %s is invalid\n", dump_dirname);
}
return num_params;
}
static void
shutdown_cerebellum(int signo)
{
carmen_warn("Inside shutdown_cerebellum\n");
carmen_cerebellum_shutdown(signo);
close_ipc();
exit(-1);
}
static void
laser_handler(void)
{
received_laser = 1;
laser_count++;
carmen_warn("%d", laser.id);
Redraw();
}
static carmen_inline void
push_state(int x, int y, double new_utility, queue state_queue)
{
state_ptr new_state = carmen_conventional_create_state(x, y, new_utility);
insert_into_queue(new_state, state_queue);
*(utility_value(x, y)) = new_utility;
if (*(costs+x*y_size+y) > 0.9)
carmen_warn("Bad utility\n");
}
void carmen_velodyne_dump_handler(char* module, char* variable, char* value) {
if (dump_file.is_open())
dump_file.close();
if (!realpath(dump_dirname, dump_path)) {
dump_path[0] = '\0';
carmen_warn("\nWarning: Dump directory %s is invalid\n", dump_dirname);
}
}
static void carmen_map_read_gridmap(carmen_FILE *fin, carmen_FILE *fout)
{
int size_x, size_y;
float resolution;
float *complete_map = NULL;
carmen_map_t map;
int chunk_type, chunk_size;
char chunk_description[12];
if(carmen_map_advance_to_chunk(fin, CARMEN_MAP_GRIDMAP_CHUNK) < 0) {
fprintf(stderr, "Error: Could not find a gridmap chunk.\n");
fprintf(stderr, " This file is probably not a map file.\n");
carmen_fclose(fin);
}
chunk_type = carmen_fgetc(fin);
carmen_fread(&chunk_size, sizeof(int), 1, fin);
carmen_fread(chunk_description, 10, 1, fin);
chunk_description[10] = '\0';
if(CARMEN_MAP_CHUNK_IS_NAMED(chunk_type)) {
if(read_string(NULL, -1, fin) < 0) {
carmen_warn("Error: Unexpected EOF.\n");
carmen_fclose(fin);
}
}
carmen_fread(&size_x, sizeof(int), 1, fin);
carmen_fread(&size_y, sizeof(int), 1, fin);
carmen_fread(&resolution, sizeof(float), 1, fin);
printf("%d %d %lf\n", size_x, size_y, resolution);
carmen_grid_mapping_create_new_map(&map, size_x, size_y, resolution);
complete_map = (float *)calloc(map.config.x_size * map.config.y_size, sizeof(float));
carmen_fread(complete_map, sizeof(float) * size_x * size_y, 1, fin);
for (int i = 0; i < map.config.x_size * map.config.y_size; i++)
{
map.complete_map[i] = complete_map[i];
}
carmen_map_write_all(fout, map.map,
map.config.x_size,
map.config.y_size,
map.config.resolution,
(char *)"", (char *)"", (char *)"", (char *)"Generated by LCAD-UFES",
NULL, 0, NULL, 0, NULL, 0);
free(complete_map);
free(map.complete_map);
free(map.map);
}
bool LogFile::load(const char* filename, bool verbose) {
char line[100000];
carmen_FILE *logfile = NULL;
carmen_logfile_index_p logfile_index = NULL;
logfile = carmen_fopen(filename, "r");
if(logfile == NULL) {
if (verbose)
carmen_warn("Error: could not open file %s for reading.\n", filename);
return false;
}
/* index the logfile */
logfile_index = carmen_logfile_index_messages(logfile);
for(int i = 0; i < logfile_index->num_messages; i++) {
/* read i-th line */
carmen_logfile_read_line(logfile_index, logfile, i, 100000, line);
/* create messages */
if(strncmp(line, "ODOM ", 5) == 0) {
push_back(new OdometryMessage(line));
}
else if(strncmp(line, "RAWLASER", 8) == 0) {
push_back(new LaserMessage(line));
}
else if(strncmp(line, "ROBOTLASER", 10) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "FLASER ", 7) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "RLASER ", 7) == 0) {
push_back(new RobotLaserMessage(line));
}
else if(strncmp(line, "TRUEPOS ", 8) == 0) {
push_back(new TrueposMessage(line));
}
else if(strncmp(line, "IMU ", 4) == 0) {
push_back(new IMUMessage(line));
}
else if(strncmp(line, "SCANMARK ", 9) == 0) {
push_back(new ScanmarkMessage(line));
}
else if(strncmp(line, "POSITIONLASER ", 14) == 0) {
push_back(new LaserposMessage(line));
}
else if(strlen(line) > 1) {
push_back(new UnknownMessage(line));
}
}
carmen_fclose(logfile);
return true;
}
carmen_ackerman_traj_point_p
carmen_planner_util_get_path_point(int index, carmen_planner_path_p path)
{
if (index >= path->length || index < 0)
{
carmen_warn("Bad path id in %s : requested %d but max id is %d.\n",
__FUNCTION__, index, path->length-1);
return NULL;
}
return path->points+index;
}
double
carmen_get_time(void)
{
struct timeval tv;
double t;
if (gettimeofday(&tv, NULL) < 0)
carmen_warn("carmen_get_time encountered error in gettimeofday : %s\n",
strerror(errno));
t = tv.tv_sec + tv.tv_usec/1000000.0;
return t;
}
static void
fill_laser_config_data(localize_ackerman_velodyne_laser_config_t *lasercfg)
{
lasercfg->num_lasers = 1 + carmen_round(lasercfg->fov / lasercfg->angular_resolution);
lasercfg->start_angle = -0.5 * lasercfg->fov;
/* give a warning if it is not a standard configuration */
if (fabs(lasercfg->fov - M_PI) > 1e-6 &&
fabs(lasercfg->fov - 100.0/180.0 * M_PI) > 1e-6 &&
fabs(lasercfg->fov - 90.0/180.0 * M_PI) > 1e-6)
carmen_warn("Warning: You are not using a standard SICK configuration (fov=%.4f deg)\n",
carmen_radians_to_degrees(lasercfg->fov));
if (fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(1.0)) > 1e-6 &&
fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(0.5)) > 1e-6 &&
fabs(lasercfg->angular_resolution - carmen_degrees_to_radians(0.25)) > 1e-6)
carmen_warn("Warning: You are not using a standard SICK configuration (res=%.4f deg)\n",
carmen_radians_to_degrees(lasercfg->angular_resolution));
}
void carmen_roadmap_plan(carmen_roadmap_t *roadmap, carmen_world_point_t *goal)
{
double dist, best_dist;
carmen_map_point_t map_goal;
int closest_node;
int i;
carmen_roadmap_vertex_t *node_list;
carmen_roadmap_edge_t *edges;
if (roadmap->nodes->length == 0)
return;
carmen_warn("Replan\n");
carmen_world_to_map(goal, &map_goal);
if (map_goal.x < 0 || map_goal.x >= goal->map->config.x_size ||
map_goal.y < 0 || map_goal.y >= goal->map->config.y_size ||
goal->map->map[map_goal.x][map_goal.y] > .001)
return;
node_list = (carmen_roadmap_vertex_t *)(roadmap->nodes->list);
best_dist = FLT_MAX;
closest_node = 0;
for (i = 0; i < roadmap->nodes->length; i++) {
node_list[i].utility = FLT_MAX;
dist = hypot(node_list[i].x-map_goal.x,node_list[i].y-map_goal.y);
if (dist < best_dist) {
best_dist = dist;
closest_node = i;
}
}
if (best_dist >= 1) {
add_node(roadmap->nodes, map_goal.x, map_goal.y);
node_list = (carmen_roadmap_vertex_t *)(roadmap->nodes->list);
roadmap->goal_id = roadmap->nodes->length-1;
construct_edges(roadmap, roadmap->goal_id);
edges = (carmen_roadmap_edge_t *)
node_list[roadmap->goal_id].edges->list;
for (i = 0; i < node_list[roadmap->goal_id].edges->length; i++) {
if (node_list[edges[i].id].edges->length) {
add_edge(node_list+roadmap->goal_id, node_list+edges[i].id,
edges[i].cost);
}
}
} else
roadmap->goal_id = closest_node;
for (i = 0; i < roadmap->nodes->length; i++)
node_list[i].utility = FLT_MAX;
}
static void strip(int argc, char *argv[])
{
char *input_filename, *output_filename, *chunk_type;
carmen_FILE *fp_in, *fp_out;
int next_arg;
int force;
next_arg = handle_options(argc, argv, &force);
next_arg++;
if(argc - next_arg != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s strip <in map filename> "
"<out map filename> <chunk type>\n\n", argv[0]);
}
input_filename = check_mapfile(argv[next_arg]);
output_filename = check_output(argv[next_arg+1], force);
chunk_type = argv[next_arg+2];
fp_in = carmen_fopen(input_filename, "r");
if(fp_in == NULL)
carmen_die_syserror("Could not open %s for reading", input_filename);
fp_out = carmen_fopen(output_filename, "w");
if(fp_out == NULL)
carmen_die_syserror("Could not open %s for writing", output_filename);
if (carmen_strcasecmp(chunk_type, "laserscans") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_LASERSCANS_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "gridmap") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_GRIDMAP_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "offlimits") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_OFFLIMITS_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "expected") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_EXPECTED_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
else if (carmen_strcasecmp(chunk_type, "places") == 0) {
if (carmen_map_strip(fp_in, fp_out, CARMEN_MAP_PLACES_CHUNK) < 0)
carmen_die_syserror("Error: could not strip file");
}
carmen_fclose(fp_in);
carmen_fclose(fp_out);
}
void
convert_depthmap_to_beams(double* beams, float fov_horiz_in_degrees, int number_of_angular_steps, float max_range,
float* depthmap, int depthmap_width, int depthmap_height, int depthmap_size)
{
int j, i = number_of_angular_steps-1;
float alpha, alpha_step, z;
float focal_length_in_pixels = ((float)depthmap_width ) / (2.0 * tan(carmen_degrees_to_radians(fov_horiz_in_degrees/ 2.0)));
int x, y = depthmap_height/2;
alpha_step = fov_horiz_in_degrees / (float) number_of_angular_steps;
for (alpha = -fov_horiz_in_degrees/2.0; alpha < fov_horiz_in_degrees/2.0; alpha += alpha_step, i--)
{
float alpha_in_radians = carmen_degrees_to_radians(alpha);
float tan_alpha_in_radians = tan(alpha_in_radians);
float cos_alpha_in_radians = cos(alpha_in_radians);
x = (int)(((float)depthmap_width)/2.0) + (int)floor(focal_length_in_pixels * tan_alpha_in_radians);
j = y * depthmap_width + x;
if (j < depthmap_size)
z = depthmap[j];
else
{
carmen_warn("Laser beams range greater than depthmap size.");
continue;
}
if (z < 0.5)
z = max_range;
if (fabs(cos_alpha_in_radians) > 1e-5)
beams[i] = z / cos_alpha_in_radians;
else
beams[i] = z;
}
if(i>0)
carmen_warn("Laser beams does not fulfill entirely FOV: total range %d, not filled range %d.\n", number_of_angular_steps, i);
}
static void tomap(int argc, char *argv[])
{
int next_arg;
int force;
double resolution;
char *input_filename, *output_filename;
carmen_map_t *map;
carmen_FILE *out_fp;
char buf[1024];
gtk_init (&argc, &argv);
next_arg = handle_options(argc, argv, &force);
if(argc - next_arg != 4) {
carmen_warn("\nError: wrong number of parameters.\n");
carmen_die("\nUsage: %s tomap <resolution> <ppm filename> "
"<map filename>\n\n", argv[0]);
}
resolution = (double)atof(argv[next_arg+1]);
if (resolution == 0)
carmen_die("%s translated to a resolution of 0.\n"
"A positive, non-zero resolution is required.\n",
argv[next_arg+1]);
input_filename = argv[next_arg+2];
if(!carmen_file_exists(input_filename))
carmen_die("Image file %s does not exist.\n", input_filename);
output_filename = check_output(argv[next_arg+3], force);
map = carmen_map_imagefile_to_map(input_filename, resolution);
out_fp = carmen_fopen(output_filename, "w");
if (carmen_map_write_id(out_fp) < 0)
carmen_die_syserror("Couldn't write map id to %s", output_filename);
sprintf(buf, "Created from %s", input_filename);
if (carmen_map_write_creator_chunk(out_fp, "img_to_map", buf) < 0)
carmen_die_syserror("Couldn't write creator chunk to %s", output_filename);
if (carmen_map_write_gridmap_chunk(out_fp, map->map, map->config.x_size,
map->config.y_size,
map->config.resolution) < 0)
carmen_die_syserror("Couldn't write gridmap chunk to %s", output_filename);
carmen_fclose(out_fp);
}
static inline double get_cost(carmen_roadmap_vertex_t *node,
carmen_roadmap_vertex_t *parent_node,
carmen_roadmap_t *roadmap)
{
carmen_roadmap_edge_t *edges;
int i, j;
int length;
double cost = 0;
carmen_map_p c_space;
c_space = roadmap->c_space;
edges = (carmen_roadmap_edge_t *)(parent_node->edges->list);
length = parent_node->edges->length;
for (i = 0; i < length; i++) {
if (edges[i].id == node->id) {
cost = edges[i].cost;
break;
}
}
if (0)
carmen_warn("Cost from %d %d to %d %d is %f\n",
parent_node->x, parent_node->y, node->x, node->y, cost);
if (edges[i].blocked)
return 1e6;
if (cost > 1e5)
return edges[i].cost;
if (carmen_dynamics_test_for_block(node, parent_node,
roadmap->avoid_people)) {
edges[i].blocked = 1;
length = parent_node->edges->length;
edges = (carmen_roadmap_edge_t *)(parent_node->edges->list);
for (j = 0; j < length; j++) {
if (edges[j].id == node->id)
break;
}
if (j < length) {
edges[j].blocked = 1;
carmen_dynamics_mark_blocked(node->id, i, parent_node->id, j);
}
return 1e6;
}
return edges[i].cost;
}
