static void
subscribe_messages(void)
{
carmen_xsens_subscribe_xsens_global_quat_message(NULL, (carmen_handler_t) xsens_mti_message_handler, CARMEN_SUBSCRIBE_LATEST);
carmen_xsens_xyz_subscribe_message(NULL, (carmen_handler_t) xsens_xyz_message_handler, CARMEN_SUBSCRIBE_LATEST);
carmen_gps_xyz_subscribe_message(NULL, (carmen_handler_t) carmen_gps_xyz_message_handler, CARMEN_SUBSCRIBE_LATEST);
carmen_gps_subscribe_nmea_hdt_message(NULL, (carmen_handler_t) gps_hdt_message_handler, CARMEN_SUBSCRIBE_LATEST);
}
void
initialize_module_args(int argc, char **argv)
{
// verifica se os parametros
// foram passados corretamente
if (argc < 2)
{
printf("Use: \n %s [bumblebee | gps | fused_odometry | kinect] [<output_path>]\n\n", argv[0]);
printf("\nOBS: output_path must be used if sensor is bumblebee or kinect.\n\n");
exit(-1);
}
char *sensor = argv[1];
// se o sensor escolhido foi a bumblebee
if (!strcmp(sensor, "bumblebee"))
{
if (argc < 3)
{
printf("\nError: You must set the output path for images!\n\n");
exit(-1);
}
output_dir_name = argv[2];
if (argc == 4)
{
if (!strcmp(argv[3], "with_gps"))
{
carmen_gps_xyz_subscribe_message(NULL, (carmen_handler_t) gps_xyz_message_handler, CARMEN_SUBSCRIBE_ALL);
gps_xyz_output_file = fopen(gps_xyz_output_filename, "w");
fprintf(gps_xyz_output_file, "timestamp,x,y\n");
}
else if (!strcmp(argv[3], "with_odometry"))
{
carmen_base_ackerman_subscribe_odometry_message(NULL, (carmen_handler_t) car_odometry_message_handler, CARMEN_SUBSCRIBE_ALL);
carmen_xsens_subscribe_xsens_global_quat_message(NULL, (carmen_handler_t) xsens_mti_message_handler, CARMEN_SUBSCRIBE_ALL);
car_odometry_output_file = fopen(car_odometry_output_filename, "w");
}
}
carmen_bumblebee_basic_subscribe_stereoimage(1, NULL, (carmen_handler_t) bumblebee_basic_handler_1, CARMEN_SUBSCRIBE_ALL);
carmen_bumblebee_basic_subscribe_stereoimage(2, NULL, (carmen_handler_t) bumblebee_basic_handler_2, CARMEN_SUBSCRIBE_ALL);
carmen_bumblebee_basic_subscribe_stereoimage(3, NULL, (carmen_handler_t) bumblebee_basic_handler_3, CARMEN_SUBSCRIBE_ALL);
carmen_bumblebee_basic_subscribe_stereoimage(4, NULL, (carmen_handler_t) bumblebee_basic_handler_4, CARMEN_SUBSCRIBE_ALL);
carmen_bumblebee_basic_subscribe_stereoimage(5, NULL, (carmen_handler_t) bumblebee_basic_handler_5, CARMEN_SUBSCRIBE_ALL);
carmen_bumblebee_basic_subscribe_stereoimage(6, NULL, (carmen_handler_t) bumblebee_basic_handler_6, CARMEN_SUBSCRIBE_ALL);
carmen_bumblebee_basic_subscribe_stereoimage(7, NULL, (carmen_handler_t) bumblebee_basic_handler_7, CARMEN_SUBSCRIBE_ALL);
carmen_bumblebee_basic_subscribe_stereoimage(8, NULL, (carmen_handler_t) bumblebee_basic_handler_8, CARMEN_SUBSCRIBE_ALL);
carmen_bumblebee_basic_subscribe_stereoimage(9, NULL, (carmen_handler_t) bumblebee_basic_handler_9, CARMEN_SUBSCRIBE_ALL);
return;
}
// se o sensor escolhido foi o gps
else if (!strcmp(sensor, "gps"))
{
carmen_gps_subscribe_nmea_message(NULL, (carmen_handler_t) gps_gpgga_handler, CARMEN_SUBSCRIBE_ALL);
gps_gpgga_output_file = fopen(gps_gpgga_output_filename, "w");
return;
}
// se o sensor escolhido foi o fused_odometry
else if (!strcmp(sensor, "fused_odometry"))
{
carmen_fused_odometry_subscribe_fused_odometry_message(NULL, (carmen_handler_t) fused_odometry_handler, CARMEN_SUBSCRIBE_ALL);
fused_odometry_output_file = fopen(fused_odometry_output_filename, "w");
return;
}
else if (!strcmp(sensor, "kinect"))
{
if (argc < 3)
{
printf("\nError: You must set kinect output_path!\n\n");
exit(-1);
}
output_dir_name = argv[2];
initialize_m_gamma ();
carmen_kinect_subscribe_depth_message(0, NULL, (carmen_handler_t) kinect_depth_handler, CARMEN_SUBSCRIBE_ALL);
carmen_kinect_subscribe_video_message(0, NULL, (carmen_handler_t) kinect_video_handler, CARMEN_SUBSCRIBE_ALL);
return;
}
else if (!strcmp(sensor, "web_cam"))
{
if (argc < 3)
{
printf("\nError: You must set web_cam output_path!\n\n");
exit(-1);
}
output_dir_name = argv[2];
carmen_web_cam_subscribe_message (NULL, (carmen_handler_t) carmen_web_cam_message_handler, CARMEN_SUBSCRIBE_ALL);
return;
}
else if (!strcmp(sensor, "stereo"))
{
if (argc < 3)
{
printf("\nError: You must set images output_path!\n\n");
exit(-1);
}
output_dir_name = argv[2];
carmen_stereo_subscribe(1, NULL, (carmen_handler_t) stereo_handler_1, CARMEN_SUBSCRIBE_ALL);
carmen_stereo_subscribe(2, NULL, (carmen_handler_t) stereo_handler_2, CARMEN_SUBSCRIBE_ALL);
carmen_stereo_subscribe(3, NULL, (carmen_handler_t) stereo_handler_3, CARMEN_SUBSCRIBE_ALL);
carmen_stereo_subscribe(4, NULL, (carmen_handler_t) stereo_handler_4, CARMEN_SUBSCRIBE_ALL);
carmen_stereo_subscribe(5, NULL, (carmen_handler_t) stereo_handler_5, CARMEN_SUBSCRIBE_ALL);
carmen_stereo_subscribe(6, NULL, (carmen_handler_t) stereo_handler_6, CARMEN_SUBSCRIBE_ALL);
carmen_stereo_subscribe(7, NULL, (carmen_handler_t) stereo_handler_7, CARMEN_SUBSCRIBE_ALL);
carmen_stereo_subscribe(8, NULL, (carmen_handler_t) stereo_handler_8, CARMEN_SUBSCRIBE_ALL);
carmen_stereo_subscribe(9, NULL, (carmen_handler_t) stereo_handler_9, CARMEN_SUBSCRIBE_ALL);
return;
}
}
