int 
main(int argc, char** argv)
{ 	
	argc_g = argc;
	argv_g = argv;
	carmen_fused_odometry_parameters fused_odometry_parameters;

	carmen_ipc_initialize(argc, argv);
	carmen_param_check_version(argv[0]);

	initialize_carmen_parameters(argc, argv, &fused_odometry_parameters);

	register_ipc_messages();

	carmen_fused_odometry_initialize(&fused_odometry_parameters);

	carmen_ipc_dispatch();

	return 0;
}
int 
main(int argc, char** argv)
{ 	
	argc_g = argc;
	argv_g = argv;
	carmen_fused_odometry_parameters fused_odometry_parameters;

	carmen_ipc_initialize(argc, argv);
	carmen_param_check_version(argv[0]);

	initialize_carmen_parameters(argc, argv, &fused_odometry_parameters);

	register_ipc_messages();

	carmen_fused_odometry_initialize(&fused_odometry_parameters);

	// carmen_ipc_addPeriodicTimer(1.0 / fused_odometry_frequency, (TIMER_HANDLER_TYPE) correct_fused_odometry_timming_and_publish, NULL);

	carmen_ipc_dispatch();
	// fused_odometry_main_loop();

	return 0;
}
Esempio n. 3
0
int main(int argc, char **argv)
{
  char filename[1024];
  char key;
  int num_scanned;

  /* initialize connection to IPC network */
  carmen_ipc_initialize(argc, argv);
  carmen_param_check_version(argv[0]);	

  /* open logfile, check if file overwrites something */
  if(argc < 2) 
    carmen_die("usage: %s <logfile>\n", argv[0]);
  sprintf(filename, "%s", argv[1]);

  outfile = carmen_fopen(filename, "r");
  if (outfile != NULL) {
    fprintf(stderr, "Overwrite %s? ", filename);
    num_scanned = scanf("%c", &key);
    if (toupper(key) != 'Y')
      exit(-1);
    carmen_fclose(outfile);
  }
  outfile = carmen_fopen(filename, "w");
  if(outfile == NULL)
    carmen_die("Error: Could not open file %s for writing.\n", filename);
  carmen_logwrite_write_header(outfile);


  get_logger_params(argc, argv);

  if  ( !(log_odometry && log_laser && log_robot_laser ) )
    carmen_warn("\nWARNING: You are neither logging laser nor odometry messages!\n");
  


  if (log_params)
    get_all_params();

  register_ipc_messages();


  if (log_odometry) 
    carmen_base_subscribe_odometry_message(NULL, (carmen_handler_t)
					   base_odometry_handler, 
					   CARMEN_SUBSCRIBE_ALL);

  if (log_sonars) 
    carmen_base_subscribe_sonar_message(NULL, (carmen_handler_t)
					   base_sonar_handler, 
					   CARMEN_SUBSCRIBE_ALL);
  if (log_bumpers) 
    carmen_base_subscribe_bumper_message(NULL, (carmen_handler_t)
					   base_bumper_handler, 
					   CARMEN_SUBSCRIBE_ALL);

  if (log_arm) 
    carmen_arm_subscribe_state_message(NULL, (carmen_handler_t)
				       arm_state_handler, 
				       CARMEN_SUBSCRIBE_ALL);

  if (log_pantilt) {
    carmen_pantilt_subscribe_scanmark_message (NULL, (carmen_handler_t)
					       pantilt_scanmark_handler, 
					       CARMEN_SUBSCRIBE_ALL);
    
    carmen_pantilt_subscribe_laserpos_message (NULL, (carmen_handler_t)
					       pantilt_laserpos_handler, 
					       CARMEN_SUBSCRIBE_ALL);
  }

  if (log_robot_laser) {
    carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)
					      robot_frontlaser_handler, 
					      CARMEN_SUBSCRIBE_ALL);
    carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)
					     robot_rearlaser_handler, 
					     CARMEN_SUBSCRIBE_ALL);
  }

  if (log_laser) {
    carmen_laser_subscribe_laser1_message(NULL, (carmen_handler_t)
					  laser_laser1_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser2_message(NULL, (carmen_handler_t)
					  laser_laser2_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser3_message(NULL, (carmen_handler_t)
					  laser_laser3_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser4_message(NULL, (carmen_handler_t)
					  laser_laser4_handler, 
					  CARMEN_SUBSCRIBE_ALL);
    carmen_laser_subscribe_laser5_message(NULL, (carmen_handler_t)
					  laser_laser5_handler, 
					  CARMEN_SUBSCRIBE_ALL);
  }

  if (log_localize) {
    carmen_localize_subscribe_globalpos_message(NULL, (carmen_handler_t)
						localize_handler,
						CARMEN_SUBSCRIBE_ALL);
  }

  if (log_simulator) {

    carmen_simulator_subscribe_truepos_message(NULL, (carmen_handler_t)  
					       carmen_simulator_truepos_handler,
					       CARMEN_SUBSCRIBE_ALL);
  }

  if (log_imu) {

    carmen_imu_subscribe_imu_message(NULL, (carmen_handler_t)  
				     imu_handler,
				     CARMEN_SUBSCRIBE_ALL);
  }
  
  
  if (log_gps) {
    carmen_gps_subscribe_nmea_message( NULL,
				       (carmen_handler_t) ipc_gps_gpgga_handler,
				       CARMEN_SUBSCRIBE_ALL );
    
    carmen_gps_subscribe_nmea_rmc_message( NULL,
					   (carmen_handler_t) ipc_gps_gprmc_handler,
					   CARMEN_SUBSCRIBE_ALL );
  }
  
  if (log_motioncmds) {
  	 carmen_robot_subscribe_vector_move_message( NULL,
  	 					 (carmen_handler_t) robot_vector_move_handler,
  	 					 CARMEN_SUBSCRIBE_ALL );
  	 carmen_robot_subscribe_follow_trajectory_message( NULL,
  	 					 (carmen_handler_t) robot_follow_trajectory_handler,
  	 					 CARMEN_SUBSCRIBE_ALL );
	 carmen_robot_subscribe_velocity_message( NULL,
	 					 (carmen_handler_t) robot_velocity_handler,
	 					 CARMEN_SUBSCRIBE_ALL );		
	 carmen_base_subscribe_velocity_message( NULL,
	 					 (carmen_handler_t) base_velocity_handler,
	 					 CARMEN_SUBSCRIBE_ALL );							 			 
  }

  carmen_logger_subscribe_comment_message( NULL,
  	 					 (carmen_handler_t) logger_comment_handler,
  	 					 CARMEN_SUBSCRIBE_ALL );

  signal(SIGINT, shutdown_module);
  logger_starttime = carmen_get_time();
  carmen_ipc_dispatch();
  return 0;
}
Esempio n. 4
0
int main(int argc, char **argv)
{
	FILE *index_file;
	char index_file_name[2000];

	memset(&odometry_ackerman, 0, sizeof(odometry_ackerman));
	memset(&velocity_ackerman, 0, sizeof(velocity_ackerman));
	
	memset(&visual_odometry, 0, sizeof(visual_odometry));
	memset(&imu, 0, sizeof(imu));
	memset(&truepos_ackerman, 0, sizeof(truepos_ackerman));
	memset(&laser_ackerman1, 0, sizeof(laser_ackerman1));
	memset(&laser_ackerman2, 0, sizeof(laser_ackerman2));
	memset(&laser_ackerman3, 0, sizeof(laser_ackerman3));
	memset(&laser_ackerman4, 0, sizeof(laser_ackerman4));
	memset(&laser_ackerman5, 0, sizeof(laser_ackerman5));
	memset(&laser_ldmrs, 0, sizeof(laser_ldmrs));
	memset(&laser_ldmrs_objects, 0, sizeof(laser_ldmrs_objects));
	memset(&laser_ldmrs_objects_data, 0, sizeof(laser_ldmrs_objects_data));
	memset(&rawlaser1, 0, sizeof(rawlaser1));
	memset(&rawlaser2, 0, sizeof(rawlaser2));
	memset(&rawlaser3, 0, sizeof(rawlaser3));
	memset(&rawlaser4, 0, sizeof(rawlaser4));
	memset(&rawlaser5, 0, sizeof(rawlaser5));
	memset(&gpsgga, 0, sizeof(gpsgga));
	memset(&gpshdt, 0, sizeof(gpshdt));
	memset(&gpsrmc, 0, sizeof(gpsrmc));
	memset(&raw_depth_kinect_0, 0, sizeof(raw_depth_kinect_0));
	memset(&raw_depth_kinect_1, 0, sizeof(raw_depth_kinect_1));
	memset(&raw_video_kinect_0, 0, sizeof(raw_video_kinect_0));
	memset(&raw_video_kinect_1, 0, sizeof(raw_video_kinect_1));
	memset(&velodyne_partial_scan, 0, sizeof(velodyne_partial_scan));
	memset(&velodyne_variable_scan, 0, sizeof(velodyne_variable_scan));
	memset(&velodyne_gps, 0, sizeof(velodyne_gps));
	memset(&xsens_euler, 0, sizeof(xsens_euler));
	memset(&xsens_quat, 0, sizeof(xsens_quat));
	memset(&xsens_matrix, 0, sizeof(xsens_matrix));
	memset(&xsens_mtig, 0, sizeof(xsens_mtig));
	memset(&bumblebee_basic_stereoimage1, 0, sizeof(bumblebee_basic_stereoimage1));
	memset(&bumblebee_basic_stereoimage2, 0, sizeof(bumblebee_basic_stereoimage2));
	memset(&bumblebee_basic_stereoimage3, 0, sizeof(bumblebee_basic_stereoimage3));
	memset(&bumblebee_basic_stereoimage4, 0, sizeof(bumblebee_basic_stereoimage4));
	memset(&bumblebee_basic_stereoimage5, 0, sizeof(bumblebee_basic_stereoimage5));
	memset(&bumblebee_basic_stereoimage6, 0, sizeof(bumblebee_basic_stereoimage6));
	memset(&bumblebee_basic_stereoimage7, 0, sizeof(bumblebee_basic_stereoimage7));
	memset(&bumblebee_basic_stereoimage8, 0, sizeof(bumblebee_basic_stereoimage8));
	memset(&bumblebee_basic_stereoimage9, 0, sizeof(bumblebee_basic_stereoimage9));
	memset(&web_cam_message, 0, sizeof(web_cam_message));
	memset(&ackerman_motion_message, 0, sizeof(ackerman_motion_message));
	memset(&ultrasonic_message, 0, sizeof(ultrasonic_message));

	carmen_ipc_initialize(argc, argv);
	carmen_param_check_version(argv[0]);

	register_ipc_messages ();
	read_parameters (argc, argv);
	carmen_playback_define_messages ();
	signal(SIGINT, shutdown_playback_module);

	logfile = carmen_fopen(argv[1], "r");
	if (logfile == NULL)
		carmen_die("Error: could not open file %s for reading.\n", argv[1]);

	int fadvise_error = posix_fadvise(fileno(logfile->fp), 0, 0, POSIX_FADV_SEQUENTIAL);
	if (fadvise_error)
	{
		printf("Could not advise POSIX_FADV_SEQUENTIAL on playback\n");
		exit(1);
	}

	strcpy(index_file_name, argv[1]);
	strcat(index_file_name, ".index");
	index_file = fopen(index_file_name, "r");
	if (index_file == NULL)
	{
		logfile_index = carmen_logfile_index_messages(logfile);
		save_logindex_file(logfile_index, index_file_name);
	}
	else if (index_file_older_than_log_file(index_file, logfile))
	{
		fclose(index_file);
		logfile_index = carmen_logfile_index_messages(logfile);
		save_logindex_file(logfile_index, index_file_name);
	}
	else
	{
		logfile_index = load_logindex_file(index_file_name);
	}

	main_playback_loop();
	return 0;
}