static void
build_points_vector_handler()
{
static int firstime = 1;
if (firstime)
{
points_vector[0].x = globalpos.globalpos.x;
points_vector[0].y = globalpos.globalpos.y;
points_vector[0].theta = globalpos.globalpos.theta;
points_vector_size = 1;
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(10), 3.0, &car_config);
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(-10), 3.0, &car_config);
points_vector_size += build_points_vector(points_vector, points_vector_size, carmen_degrees_to_radians(0), 3.0, &car_config);
printf("enviando %d, pose %f %f %f\n", points_vector_size, globalpos.globalpos.x, globalpos.globalpos.y, globalpos.globalpos.theta);
carmen_motion_planner_publish_path_message(points_vector, points_vector_size, 0);
// publish_navigator_ackerman_plan_message(points_vector, points_vector_size);
// send_base_ackerman_command(points_vector, points_vector_size);
carmen_ipc_sleep(0.5);
firstime = 0;
}
}
void
Timer(int value __attribute__ ((unused)))
{
carmen_ipc_sleep(0.01);
glutPostWindowRedisplay(ultrasonic_window_id);
glutTimerFunc(redraw_update_period, Timer, 1);
}
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_Thread::updateIPC(int i)
{
carmen_ipc_sleep(0.01);
carmen_graphics_update_ipc_callbacks_qt(this, SLOT(updateIPC(int)));
i = 1; // only to make the compiler happy
return i;
}
void
fused_odometry_main_loop(void)
{
while (1)
{
correct_fused_odometry_timming_and_publish();
carmen_ipc_sleep(1.0 / fused_odometry_frequency);
}
}
int
main(int argc, char *argv[])
{
int gps_nr = 0;
SerialDevice dev;
carmen_gps_gpgga_message gpgga;
carmen_gps_gprmc_message gprmc;
carmen_erase_structure(&gpgga, sizeof(carmen_gps_gpgga_message) );
carmen_erase_structure(&gprmc, sizeof(carmen_gps_gprmc_message) );
gpgga.host = carmen_get_host();
gprmc.host = carmen_get_host();
DEVICE_init_params( &dev );
carmen_ipc_initialize( argc, argv );
ipc_initialize_messages();
read_parameters( &dev, argc, argv );
carmen_extern_gpgga_ptr = &gpgga;
carmen_extern_gpgga_ptr->nr = gps_nr;
carmen_extern_gprmc_ptr = &gprmc;
carmen_extern_gprmc_ptr->nr = gps_nr;
fprintf( stderr, "INFO: ************************\n" );
fprintf( stderr, "INFO: ********* GPS ********\n" );
fprintf( stderr, "INFO: ************************\n" );
fprintf( stderr, "INFO: open device: %s\n", dev.ttyport );
if (DEVICE_connect_port( &dev )<0) {
fprintf( stderr, "ERROR: can't open device !!!\n\n" );
exit(1);
} else {
fprintf( stderr, "INFO: done\n" );
}
while(TRUE) {
if ( DEVICE_bytes_waiting( dev.fd )>10 ) {
if (DEVICE_read_data( dev )) {
gpgga.timestamp = carmen_get_time();
gprmc.timestamp = carmen_get_time();
ipc_publish_position();
}
usleep(100000);
} else {
carmen_ipc_sleep(0.25);
//IPC_listen(0);
//usleep(250000);
}
}
return(0);
}
void* velodyne_listen(void* p) {
pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, 0);
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, 0);
while (!quit) {
carmen_ipc_sleep(0.0);
pthread_testcancel();
}
return 0;
}
int main(int argc, char** argv) {
int c;
carmen_ipc_initialize(argc,argv);
carmen_param_check_version(argv[0]);
signal(SIGINT, shutdown_module);
if (argc > 1 && !strcmp(argv[1], "-sim"))
simulation = 1;
if (carmen_map_get_hmap(&hmap) < 0)
carmen_die("no hmap found\n");
print_hmap(); //dbug
carmen_map_change_map_zone(hmap.zone_names[0]); //dbug: this should go in the map server
map_zone = 0;
ipc_init();
carmen_terminal_cbreak(0);
while(1) {
carmen_ipc_sleep(0.01);
c = getchar();
/*
if (c == EOF)
break;
*/
if (c != 27)
continue;
c = getchar();
/*
if (c == EOF)
break;
*/
if (c != 91)
continue;
c = getchar();
/*
if (c == EOF)
break;
*/
switch(c) {
case 65: level_up(); break;
case 66: level_down();
}
}
return 0;
}
int
main(int argc, char **argv)
{
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
carmen_robot_ackerman_start(argc, argv);
signal(SIGINT, shutdown_robot_ackerman);
while (1)
{
carmen_ipc_sleep(0.1);
carmen_robot_ackerman_run();
}
return 0;
}
int main(int argc, char **argv)
{
double command_tv = 0.0, command_rv = 0.0;
char c;
int quit = 0;
double f_timestamp;
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
// Initialize keybord, joystick and IPC
carmen_initialize_keyboard();
if (carmen_initialize_joystick(&joystick) >= 0)
no_joystick = 0;
read_parameters(argc, argv);
signal(SIGINT, sig_handler);
f_timestamp = carmen_get_time();
while(quit>=0) {
carmen_ipc_sleep(0.05);
if(!no_joystick && carmen_get_joystick_state(&joystick) >= 0) {
carmen_joystick_control(&joystick, max_tv, max_rv,
&command_tv, &command_rv);
carmen_robot_velocity_command(command_tv, command_rv);
f_timestamp = carmen_get_time();
}
else if(carmen_read_char(&c)) {
quit = carmen_keyboard_control(c, max_tv, max_rv,
&command_tv, &command_rv);
carmen_robot_velocity_command(command_tv, command_rv);
f_timestamp = carmen_get_time();
}
else if(carmen_get_time() - f_timestamp > 0.5) {
carmen_robot_velocity_command(command_tv, command_rv);
f_timestamp = carmen_get_time();
}
}
sig_handler(SIGINT);
return 0;
}
static void level_down() {
carmen_hmap_link_p link;
int i;
printf("\nlevel_down()\n");
link = get_closest_link(CARMEN_HMAP_LINK_ELEVATOR);
if (link == NULL || link->keys[0] == map_zone)
return;
for (i = 0; i < link->degree; i++)
if (link->keys[i] == map_zone)
break;
carmen_map_change_map_zone(hmap.zone_names[link->keys[i-1]]);
carmen_ipc_sleep(1.0);
set_robot_pose(transform_robot_pose(link->points[i], link->points[i-1]));
}
void carmen_multicentral_ipc_sleep(carmen_centrallist_p centrallist,
double sleep_time)
{
int i, count = 0;
for(i = 0; i < centrallist->num_centrals; i++)
if(centrallist->central[i].connected)
count++;
if(count == 0)
usleep((int)(sleep_time * 1e6));
else {
/* handle IPC messages for each central */
for(i = 0; i < centrallist->num_centrals; i++)
if(centrallist->central[i].connected) {
IPC_setContext(centrallist->central[i].context);
carmen_ipc_sleep(sleep_time / count);
}
}
}
int main(int argc, char **argv)
{
carmen_camera_image_t *image;
IPC_RETURN_TYPE err;
double interframe_sleep = 5.0;
int param_err;
/* connect to IPC server */
carmen_ipc_initialize(argc, argv);
carmen_param_check_version(argv[0]);
err = IPC_defineMsg(CARMEN_CAMERA_IMAGE_NAME, IPC_VARIABLE_LENGTH,
CARMEN_CAMERA_IMAGE_FMT);
carmen_test_ipc_exit(err, "Could not define", CARMEN_CAMERA_IMAGE_NAME);
carmen_param_allow_unfound_variables(0);
param_err = carmen_param_get_double("camera_interframe_sleep", &interframe_sleep, NULL);
if (param_err < 0)
carmen_die("Could not find parameter in carmen.ini file: camera_interframe_sleep\n");
image = carmen_camera_start(argc, argv);
if (image == NULL)
exit(-1);
signal(SIGINT, shutdown_module);
while(1) {
carmen_camera_grab_image(image);
if(image->is_new) {
carmen_camera_publish_image_message(image);
carmen_warn("c");
image->is_new = 0;
}
// We are rate-controlling the camera.
carmen_publish_heartbeat("camera_daemon");
carmen_ipc_sleep(interframe_sleep);
}
return 0;
}
void
carmen_localize_ackerman_initialize_particles_uniform(carmen_localize_ackerman_particle_filter_p filter,
carmen_robot_ackerman_laser_message *laser,
carmen_localize_ackerman_map_p map)
{
priority_queue_p queue = priority_queue_init(filter->param->num_particles);
double *laser_x, *laser_y;
int i, j, x_l, y_l;
float angle, prob, ctheta, stheta;
carmen_point_t point;
queue_node_p mark;
int *beam_valid;
/* compute the correct laser_skip */
if (filter->param->laser_skip <= 0) {
filter->param->laser_skip =
floor(filter->param->integrate_angle / laser->config.angular_resolution);
}
fprintf(stderr, "\rDoing global localization... (%.1f%% complete)", 0.0);
filter->initialized = 0;
/* copy laser scan into temporary memory */
laser_x = (double *)calloc(laser->num_readings, sizeof(double));
carmen_test_alloc(laser_x);
laser_y = (double *)calloc(laser->num_readings, sizeof(double));
carmen_test_alloc(laser_y);
beam_valid = (int *)calloc(laser->num_readings, sizeof(int));
carmen_test_alloc(beam_valid);
for(i = 0; i < laser->num_readings; i++) {
if (laser->range[i] < laser->config.maximum_range &&
laser->range[i] < filter->param->max_range)
beam_valid[i] = 1;
else
beam_valid[i] = 0;
}
/* do all calculations in map coordinates */
for(i = 0; i < laser->num_readings; i++) {
angle = laser->config.start_angle +
i * laser->config.angular_resolution;
laser_x[i] = (filter->param->front_laser_offset +
laser->range[i] * cos(angle)) / map->config.resolution;
laser_y[i] = (laser->range[i] * sin(angle)) / map->config.resolution;
}
for(i = 0; i < filter->param->global_test_samples; i++) {
if(i % 10000 == 0)
{
fprintf(stderr, "\rDoing global localization... (%.1f%% complete)",
i / (double)filter->param->global_test_samples * 100.0);
carmen_ipc_sleep(0.001);
}
do {
point.x = carmen_uniform_random(0, map->config.x_size - 1);
point.y = carmen_uniform_random(0, map->config.y_size - 1);
} while(map->carmen_map.map[(int)point.x][(int)point.y] >
filter->param->occupied_prob ||
map->carmen_map.map[(int)point.x][(int)point.y] == -1);
point.theta = carmen_uniform_random(-M_PI, M_PI);
prob = 0.0;
ctheta = cos(point.theta);
stheta = sin(point.theta);
for(j = 0; j < laser->num_readings &&
(queue->last == NULL || prob > queue->last->prob);
j += filter->param->laser_skip)
{
if (beam_valid[j]) {
x_l = point.x + laser_x[j] * ctheta - laser_y[j] * stheta;
y_l = point.y + laser_x[j] * stheta + laser_y[j] * ctheta;
if(x_l >= 0 && y_l >= 0 && x_l < map->config.x_size &&
y_l < map->config.y_size)
prob += map->gprob[x_l][y_l];
else
prob -= 100;
}
}
priority_queue_add(queue, point, prob);
}
/* transfer samples from priority queue back into particles */
mark = queue->first;
for(i = 0; i < queue->num_elements; i++) {
filter->particles[i].x = (mark->point.x * map->config.resolution) + map->config.x_origin;
filter->particles[i].y = (mark->point.y * map->config.resolution) + map->config.y_origin;
filter->particles[i].theta = mark->point.theta;
mark = mark->next;
}
priority_queue_free(queue);
free(laser_x);
free(laser_y);
free(beam_valid);
if(filter->param->do_scanmatching) {
for(i = 0; i < filter->param->num_particles; i++) {
point.x = filter->particles[i].x;
point.y = filter->particles[i].y;
point.theta = filter->particles[i].theta;
carmen_localize_ackerman_laser_scan_gd(laser->num_readings, laser->range,
laser->config.angular_resolution,
laser->config.start_angle,
&point,
filter->param->front_laser_offset,
map,
filter->param->laser_skip);
filter->particles[i].x = point.x;
filter->particles[i].y = point.y;
filter->particles[i].theta = point.theta;
filter->particles[i].weight = 0.0;
}
}
filter->initialized = 1;
filter->first_odometry = 1;
filter->global_mode = 1;
filter->distance_travelled = 0;
fprintf(stderr, "\rDoing global localization... (%.1f%% complete)\n\n",
100.0);
}
int main(int argc, char *argv[]) {
UDPConnectionServer connection(2368);
AcquisitionThread<DataPacket> thread(connection);
Calibration calib;
std::shared_ptr<DataPacket> packet;
IPC_RETURN_TYPE err;
char dump_filename[4096];
carmen_velodyne_ipc_initialize(argc, argv);
carmen_velodyne_read_parameters(argc, argv);
carmen_velodyne_set_spin_rate(spin_rate);
calib_file.open(calib_path);
if (!calib_file.is_open())
carmen_die("\nError: Could not open %s for reading\n", calib_filename);
calib_file >> calib;
calib_file.close();
signal(SIGINT, carmen_velodyne_sigint_handler);
thread.start();
double time;
double start_time = carmen_get_time();
int num_packets = 0;
int start_num_packets = 0;
int num_lost_packets = 0;
int start_num_lost_packets = 0;
while (!quit) {
try {
while (dump_enabled || points_publish) {
packet = thread.getBuffer().dequeue();
++num_packets;
if (dump_enabled) {
if (!dump_file.is_open()) {
char dump_time[1024];
time_t local = packet->getTimestamp();
struct tm* time = localtime(&local);
strftime(dump_time, sizeof(dump_time), "%Y-%m-%d-%H%M%S", time);
sprintf(dump_filename, "%s/%s.bin", dump_path, dump_time);
dump_file.open(dump_filename, std::ios::out | std::ios::binary);
}
if (dump_file.is_open()) {
long dump_filepos = dump_file.tellp();
dump_file << *packet;
carmen_velodyne_publish_packet(id, dump_filename,
dump_filepos, packet->getTimestamp());
}
else {
carmen_warn("\nWarning: Could not open %s for writing\n",
dump_filename);
}
}
if (points_publish) {
VdynePointCloud pointCloud;
Converter::toPointCloud(*packet, calib, pointCloud);
int num_points = pointCloud.getSize();
float x[num_points], y[num_points], z[num_points];
std::vector<VdynePointCloud::Point3D>::const_iterator it;
int i = 0;
for (it = pointCloud.getPointBegin();
it != pointCloud.getPointEnd(); ++it, ++i) {
x[i] = it->mX;
y[i] = it->mY;
z[i] = it->mZ;
}
carmen_velodyne_publish_pointcloud(id, num_points, x, y, z,
packet->getTimestamp());
}
double time = carmen_get_time();
if (time-start_time >= 1.0) {
num_lost_packets = thread.getBuffer().getNumDroppedElements();
float period_num_packets = num_packets-start_num_packets;
float period_num_lost_packets = num_lost_packets-
start_num_lost_packets;
double packet_loss = period_num_lost_packets/
(period_num_packets+period_num_lost_packets);
fprintf(stdout, "\rPacket loss: %6.2f%%", packet_loss*100.0);
fflush(stdout);
start_time = time;
start_num_packets = num_packets;
start_num_lost_packets = num_lost_packets;
}
}
}
catch (IOException& exception) {
// buffer underrun
}
carmen_ipc_sleep(0.0);
}
if (dump_file.is_open())
dump_file.close();
fprintf(stdout, "\n");
return 0;
}
static void hmap_warp_check(carmen_point_t old_pose, carmen_point_t new_pose) {
int i;
double x1, x2, x3, x4, y1, y2, y3, y4;
static int warping = 0;
static char *warp_zone;
static carmen_point_t warp_pose;
static carmen_point_t warp_src;
static carmen_point_t warp_dst;
static carmen_point_t warp_post1;
static carmen_point_t warp_post2;
if (warping) {
if (line_side_test(warp_post1, warp_post2, warp_pose, new_pose) < 0) {
if (carmen_distance(&warp_pose, &new_pose) > 0.5) { //dbug: param?
carmen_map_change_map_zone(warp_zone);
carmen_ipc_sleep(1.0);
set_robot_pose(transform_robot_pose(warp_src, warp_dst));
warping = 0;
}
}
else
warping = 0;
}
else {
for (i = 0; i < hmap.num_links; i++) {
if (hmap.links[i].type == CARMEN_HMAP_LINK_DOOR) {
if (hmap.links[i].keys[0] == map_zone) {
if (segment_intersect(old_pose, new_pose, hmap.links[i].points[0], hmap.links[i].points[1])) {
warping = 1;
warp_zone = hmap.zone_names[hmap.links[i].keys[1]];
warp_pose = old_pose;
x1 = hmap.links[i].points[0].x;
x2 = hmap.links[i].points[1].x;
x3 = hmap.links[i].points[2].x;
x4 = hmap.links[i].points[3].x;
y1 = hmap.links[i].points[0].y;
y2 = hmap.links[i].points[1].y;
y3 = hmap.links[i].points[2].y;
y4 = hmap.links[i].points[3].y;
warp_src.x = (x1 + x2) / 2.0;
warp_src.y = (y1 + y2) / 2.0;
warp_src.theta = atan2(y2-y1, x2-x1);
warp_dst.x = (x3 + x4) / 2.0;
warp_dst.y = (y3 + y4) / 2.0;
warp_dst.theta = atan2(y4-y3, x4-x3);
warp_post1 = hmap.links[i].points[0];
warp_post2 = hmap.links[i].points[1];
}
}
else if (hmap.links[i].keys[1] == map_zone) {
if (segment_intersect(old_pose, new_pose, hmap.links[i].points[2], hmap.links[i].points[3])) {
warping = 1;
warp_zone = hmap.zone_names[hmap.links[i].keys[0]];
warp_pose = old_pose;
x1 = hmap.links[i].points[0].x;
x2 = hmap.links[i].points[1].x;
x3 = hmap.links[i].points[2].x;
x4 = hmap.links[i].points[3].x;
y1 = hmap.links[i].points[0].y;
y2 = hmap.links[i].points[1].y;
y3 = hmap.links[i].points[2].y;
y4 = hmap.links[i].points[3].y;
warp_dst.x = (x1 + x2) / 2.0;
warp_dst.y = (y1 + y2) / 2.0;
warp_dst.theta = atan2(y2-y1, x2-x1);
warp_src.x = (x3 + x4) / 2.0;
warp_src.y = (y3 + y4) / 2.0;
warp_src.theta = atan2(y4-y3, x4-x3);
warp_post1 = hmap.links[i].points[2];
warp_post2 = hmap.links[i].points[3];
}
}
}
}
}
}
void main_playback_loop(void)
{
print_playback_status();
while (1)
{
// eu faco esse teste para evitar uma sobrecarga de mensagens sobre o central e o
// playback_control. vale ressaltar que essa mensagem so possuir carater informativo
if (fabs(timestamp_last_message_published - playback_timestamp) > 0.05)
publish_info_message();
if(advance_frame || rewind_frame)
paused = 1;
if(offset != 0) {
playback_starttime = 0.0;
current_position += offset;
if(current_position < 0)
current_position = 0;
if(current_position >= logfile_index->num_messages - 1)
current_position = logfile_index->num_messages - 2;
offset = 0;
}
if(!paused && current_position >= logfile_index->num_messages - 1) {
paused = 1;
current_position = 0;
playback_starttime = 0.0;
playback_timestamp = 0;
print_playback_status();
}
else if(!paused && current_position < logfile_index->num_messages - 1) {
read_message(current_position, 1, 0);
current_position++;
}
else if(paused && advance_frame) {
// laser = 0;
// while(current_position < logfile_index->num_messages - 1 && !laser) {
// laser = read_message(current_position, 1);
// current_position++;
// }
if (current_position < logfile_index->num_messages - 1) {
read_message(current_position, 1, 0);
current_position++;
publish_info_message();
}
print_playback_status();
advance_frame = 0;
}
else if(paused && rewind_frame) {
// laser = 0;
// while(current_position > 0 && !laser) {
// current_position--;
// laser = read_message(current_position, 0);
// }
// laser = 0;
// while(current_position > 0 && !laser) {
// current_position--;
// laser = read_message(current_position, 0);
// }
// read_message(current_position, 1);
// current_position++;
if (current_position > 0) {
current_position--;
read_message(current_position, 1, 0);
publish_info_message();
}
print_playback_status();
rewind_frame = 0;
}
if(paused)
carmen_ipc_sleep(0.01);
if(fast)
carmen_ipc_sleep(0.000001);
else
carmen_ipc_sleep(0.0001);
}
}
