Esempio n. 1
0
    void operator ()(parse_state &state, int argc, char **argv)
    {
      state.prog = argv[0];
      --argc, ++argv;
      if(argc == 0)
      { throw helper{ state.prog }; }

      std::vector<std::string> args;
      std::copy(argv, argv + argc, std::back_inserter(args));

      auto const end(args.end());
      for(auto i(args.begin()); i != end; ++i)
      {
        bool read{};
        read |= param_help(state, i, end);
        if(!read)
        { read |= param_version(state, i, end); }
        if(!read)
        { read |= param_verbosity(state, i, end); }
        if(!read)
        { read |= param_output(state, i, end); }
        if(!read)
        { read |= param_input(state, i, end); }

        if(!read)
        { throw invalid_cmdline(state.prog, "unknown argument: " + *i); }
      }

      if(state.input_file.empty())
      { throw helper{ state.prog }; }
      if(state.verbose)
      { std::cout << "input: " << state.input_file << std::endl; }
    }
Esempio n. 2
0
/**
 * Main function
 */
int main(int argc, char **argv)
{
    int exit_code = 0;

    if (argc == 2) {
	// Process source file
	process_file(argv[1]);
	if (get_error()) {
	    if (get_error() == ERROR_LEX) {
		exit_code = 1;
		fprintf(stderr, "Error in lexical analyse\n");
	    } else if (get_error() == ERROR_SYNTAX) {
		exit_code = 2;
		fprintf(stderr, "Error in syntax analyse\n");
	    } else if (get_error() == ERROR_SEMANTIC) {
		exit_code = 3;
		fprintf(stderr, "Error in semantic analyse\n");
	    } else if (get_error() == ERROR_INTERPRETER) {
		exit_code = 4;
		fprintf(stderr, "Error in interpreter\n");
	    } else {
		// Other error
		exit_code = 255;
		switch (get_error()) {
		    case ERROR_NO_MEMORY:
			fprintf(stderr, "Not enough memory\n");
			break;
		    case ERROR_FOPEN:
			fprintf(stderr, "File opening error\n");
			break;
		    case ERROR_STACK_POP:
			fprintf(stderr, "Stack pop error\n");
			break;
		    case ERROR_STACK_TOP:
			fprintf(stderr, "Stack top error\n");
			break;
		    case ERROR_STACK_LOOKATNEXT:
			fprintf(stderr, "Stack lookatnext error\n");
			break;
		    case ERROR_THREEADR_CODE:
			fprintf(stderr, "Three addr code error\n");
			break;
		}
	    }
	}
    } else {
	// Show parameter help
	param_help();
    }

    exit(exit_code);
}
Esempio n. 3
0
/* main */
int main( int argc, char *argv[] )
{
	pthread_t command_thread;
	pthread_t control_thread;
	pthread_t update_thread;
	int command_thread_en;
	int control_thread_en;
	int update_thread_en;
	Ver_t version;
	Param_t driver_param;
	int i, ret;
	ParametersPtr param;
	char paramfile[512];
	int quit;

	hook_pre_global();

	ret = arg_analyze( argc, argv );
	if( option( OPTION_DAEMON ) )
	{
#if HAVE_FORK
		pid_t pid;

		pid = fork(  );
		if( pid < 0 )
		{
			return -1;
		}
		else if( pid != 0 )
		{
			return 0;
		}
		setsid(  );
		if( chdir( "/" ) < 0 )
		{
			yprintf( OUTPUT_LV_ERROR, "Failed to chdir.\n" );
			return EXIT_FAILURE;
		}
		close( STDIN_FILENO );
		close( STDOUT_FILENO );
		close( STDERR_FILENO );
#else
		yprintf( OUTPUT_LV_ERROR, "Daemon mode is not supported in your system.\n" );
		return EXIT_FAILURE;
#endif
	}
	if( option( OPTION_SHOW_HELP ) )
	{
		arg_help( argc, argv );
		return EXIT_SUCCESS;
	}
	if( option( OPTION_SHOW_LONGHELP ) )
	{
		arg_longhelp( argc, argv );
		return EXIT_SUCCESS;
	}
	if( option( OPTION_SHOW_PARAMHELP ) )
	{
		param_help(  );
		return EXIT_SUCCESS;
	}
	if( option( OPTION_VERSION ) )
	{
		fprintf( stderr, "YamabicoProject-Spur\n" );
		fprintf( stderr, " Ver. %s\n", PACKAGE_VERSION );
		return EXIT_SUCCESS;
	}
	if( !ret )									/* オプション解析に失敗したとき */
		return EXIT_FAILURE;

	yprintf( OUTPUT_LV_PROCESS, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
	yprintf( OUTPUT_LV_PROCESS, "YamabicoProject-Spur\n" );
	yprintf( OUTPUT_LV_PROCESS, " Ver. %s\n", PACKAGE_VERSION );
	yprintf( OUTPUT_LV_PROCESS, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );

	/* Ctrl-C割り込みハンドラーの登録 */
	escape_road(  );
	g_emergency = 0;

	/* パラメータを読み込み、セットする */
	param = get_param_ptr(  );

#ifdef HAVE_SSM
	/* SSM初期化 */
	if( !option( OPTION_WITHOUT_SSM ) )
		init_ypspurSSM( param->ssm_id );
#endif

	/* 座標系の初期化、コマンド処理系の初期化 */
	init_coordinate_systems(  );
	init_odometry(  );
	init_spur_command(  );

	fflush( stderr );

	command_thread_en = 0;
	command_thread_en = 0;
	do
	{
		FILE *temp_paramfile = NULL;
		quit = 0;

		yprintf( OUTPUT_LV_PROCESS, "Device Information\n" );
		yprintf( OUTPUT_LV_PROCESS, " Port    : %s \n", param->device_name );

		if( !( option( OPTION_WITHOUT_DEVICE ) ) )
		{
			if( !serial_connect( param->device_name ) )
			{
				// quit=0;でbreakしたら異常終了と判断される
				break;
			}
			if( !( option( OPTION_DO_NOT_USE_YP ) ) )
			{
				int current, age;
				sscanf( YP_PROTOCOL_NAME, "YPP:%d:%d", &current, &age );

				yprintf( OUTPUT_LV_PROCESS, " Checking device information...\r" );
				for ( i = 0; i < 3; i++ )
				{
					int device_current, device_age;
					// プロトコルがYPであることを確認
					if( get_version( &version ) == -1 )
					{
						continue;
					}
					if( strstr( version.protocol, "YPP:" ) != version.protocol )
					{
						continue;
					}
					sscanf( version.protocol, "YPP:%d:%d", &device_current, &device_age );
					if( device_current - device_age > current || device_current < current )
					{
						continue;
					}
					break;
				}
				yprintf( OUTPUT_LV_PARAM, " Vender  : %s\033[K\n", version.vender );
				yprintf( OUTPUT_LV_PARAM, " Product : %s\n", version.product );
				yprintf( OUTPUT_LV_PARAM, " Firmware: %s\n", version.firmware );
				yprintf( OUTPUT_LV_PARAM, " Protcol : %s\n", version.protocol );
				yprintf( OUTPUT_LV_PARAM, " Serialno: %s\n", version.serialno );
				yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
				if( i == 3 )
				{
					yprintf( OUTPUT_LV_ERROR, "Error: Device doesn't have available YP protocol version.\n" );
					if( option( OPTION_RECONNECT ) && g_emergency == 0 )
					{
						yp_usleep( 500000 );
						continue;
					}
					break;						// quit=0でbreakしたら異常終了と判断
				}
			}
			fflush( stderr );

			if( get_parameter( &driver_param ) == -1 )
			{
				continue;
			}
			yprintf( OUTPUT_LV_PARAM, "Driver depending parameters\n" );
			yprintf( OUTPUT_LV_PARAM, " Name          : %s\n", driver_param.robot_name );
			yprintf( OUTPUT_LV_PARAM, " PWM resolution: %s\n", driver_param.pwm_resolution );
			yprintf( OUTPUT_LV_PARAM, " Motor number  : %s\n", driver_param.motor_num );
			yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );

			if( strlen( driver_param.pwm_resolution ) <= 0 || 
			    strlen( driver_param.motor_num ) <= 0 )
			{
				yprintf( OUTPUT_LV_ERROR, "Error: Failed to load driver parameters.\n" );
				if( option( OPTION_RECONNECT ) && g_emergency == 0 )
				{
					yp_usleep( 500000 );
					continue;
				}
				break;
			}
		}
		if( !( option( OPTION_PARAM_FILE ) ) )
		{
			// パラメータファイルが指定されておらず、ドライバにパラメータが内蔵されている場合
			if( strcmp( driver_param.robot_name, "embedded" ) == 0 )
			{
				char param[2048];

				yprintf( OUTPUT_LV_MODULE, "Reading device embedded parameter.\n" );
				temp_paramfile = tmpfile(  );
				if( !temp_paramfile )
				{
					yprintf( OUTPUT_LV_ERROR, "Error: Failed to create temporary file.\n" );
					return 0;
				}
				if( !get_embedded_param( param ) )
				{
					yprintf( OUTPUT_LV_ERROR, "Error: Failed to read embedded parameters.\n" );
					if( option( OPTION_RECONNECT ) && g_emergency == 0 )
					{
						yp_usleep( 500000 );
						continue;
					}
					break;
				}
				fprintf( temp_paramfile, "%s", param );
				fseek( temp_paramfile, 0L, SEEK_SET );
			}
			// パラメータファイルが指定されておらず、ドライバにロボット名が登録されている場合
			else if( strlen( driver_param.robot_name ) > 0 && strcmp( driver_param.robot_name, "unknown" ) != 0 )
			{
				strcpy( param->parameter_filename, driver_param.robot_name );
				strcat( param->parameter_filename, ".param" );
			}
		}
		if( temp_paramfile )
		{
			yprintf( OUTPUT_LV_PARAM, "Embedded parameter file\n" );
			if( !set_paramptr( temp_paramfile ) )
			{
				yprintf( OUTPUT_LV_ERROR, "Error: Cannot use embedded parameter.\n" );
				return 0;
			}
		}
		else
		{
			yprintf( OUTPUT_LV_PARAM, "Parameter file: %s\n", param->parameter_filename );
			if( !set_param( param->parameter_filename, paramfile ) )
			{
				yprintf( OUTPUT_LV_ERROR, "Error: Cannot find parameter file.\n" );
				return 0;
			}
		}
		{
			int i;
			for ( i = 0; i < YP_PARAM_MOTOR_NUM; i++ )
			{
				*pp( YP_PARAM_PWM_MAX, i ) = atoi( driver_param.pwm_resolution );
			}

		}
		yprintf( OUTPUT_LV_PARAM, "++++++++++++++++++++++++++++++++++++++++++++++++++\n\n" );

		if( !( option( OPTION_WITHOUT_DEVICE ) ) )
		{
			// ボーレートの設定
			if( param->speed )
			{
				yprintf( OUTPUT_LV_MODULE, "Setting baudrate to %d baud.\n", param->speed );
			}
			else {
				// 指定されてない場合デフォルトの値
				param->speed = DEFAULT_BAUDRATE;
			}

			ret = set_baudrate( param->speed );
			if( ret == 0 )
			{
				// 設定失敗
				yprintf( OUTPUT_LV_WARNING, "Error: Failed to change baudrate.\n" );

				serial_close(  );

				quit = 0;
				break;						// quit=0でbreakしたら異常終了と判断
			}
			if (ret == 4)
			{
				// ボーレートの設定未対応
				yprintf( OUTPUT_LV_WARNING, "Warn: Baudrate setting is not supported on this device.\n" );
			}
			else 
			{
				// 設定成功
				// 正常ならば何もしない
			}

			if( param->admask )
			{
				yprintf( OUTPUT_LV_MODULE, "Setting admask to %x.\n", param->admask );
				set_admask( param->admask );
			}

			if( option( OPTION_ENABLE_GET_DIGITAL_IO ) )
			{
				yprintf( OUTPUT_LV_MODULE, "Enabling digital io input.\n" );
				set_diomask( 1 );
			}

			if( !( option( OPTION_PARAM_CONTROL ) ) )
			{
				apply_robot_params(  );
			}

			/* サーボをかける */
			if( state( YP_STATE_MOTOR ) && state( YP_STATE_VELOCITY ) )
			{
				motor_servo(  );
			}
		}

		yprintf( OUTPUT_LV_MODULE, "YP-Spur coordinator started.\n" );

		/* スレッド初期化 */
		init_command_thread( &command_thread );
		pthread_detach( command_thread );
		command_thread_en = 1;

		if( !( option( OPTION_WITHOUT_DEVICE ) ) )
		{
			init_control_thread( &control_thread );
			pthread_detach( control_thread );
			control_thread_en = 1;
		}
		if( option( OPTION_UPDATE_PARAM ) )
		{
			init_param_update_thread( &update_thread, paramfile );
			pthread_detach( update_thread );
			update_thread_en = 1;
		}

		// オドメトリ受信ループ
#if HAVE_SIGLONGJMP
		if( sigsetjmp( ctrlc_capture, 1 ) != 0 )
		{
			quit = 1;
		}
		else
#elif HAVE_LONGJMP
		if( setjmp( ctrlc_capture ) != 0 )
		{
			quit = 1;
		}
		else
#endif
		{
			if( !( option( OPTION_WITHOUT_DEVICE ) ) )
			{
				odometry_receive_loop(  );
			}
			else
			{
				while( 1 )
					yp_usleep( 1000000 );
			}
			yprintf( OUTPUT_LV_MODULE, "Connection to %s was closed.\n", param->device_name );
		}

		/* 終了処理 */
		if( !( option( OPTION_WITHOUT_DEVICE ) ) )
		{
			serial_close(  );
		}

		if( update_thread_en )
		{
			pthread_cancel( update_thread );
			pthread_join( update_thread, NULL );
			update_thread_en = 0;
		}
		if( control_thread_en )
		{
			pthread_cancel( control_thread );
			pthread_join( control_thread, NULL );
			control_thread_en = 0;
		}
		if( command_thread_en )
		{
			pthread_cancel( command_thread );
			pthread_join( command_thread, NULL );
			command_thread_en = 0;
		}

		if( option( OPTION_RECONNECT ) && quit == 0 )
		{
			init_spur_command(  );
			yp_usleep( 500000 );
			if( !( option( OPTION_WITHOUT_DEVICE ) ) )
			{
				while( !serial_tryconnect( param->device_name ) )
				{
					yp_usleep( 200000 );
				}
			}
			yprintf( OUTPUT_LV_MODULE, "++++++++++++++++++++++++++++++++++++++++++++++++++\n" );
			yp_usleep( 500000 );
			continue;
		}
		break;
	}
	while( 1 );

#ifdef HAVE_SSM
	/* SSM終了処理 */
	if( !option( OPTION_WITHOUT_SSM ) )
		end_ypspurSSM(  );
#endif

	yp_usleep( 200000 );
	fflush( stderr );

	return ( quit ? EXIT_SUCCESS : EXIT_FAILURE );
}