int main( int argc, char **argv )
{
hook_pre_global();
if( argc > 1 )
{
char ip[64];
char *p;
int port;
strncpy( ip, argv[1], 64 );
p = strchr( ip, ':' );
if( p == NULL )
{
fprintf( stderr, "USAGE: %s ip:port\n", argv[0] );
return -1;
}
*p = 0;
port = atoi( p + 1 );
Spur_init_socket( ip, port );
}
else
{
if( Spur_init( ) < 0 )
{
fprintf( stderr, "ERROR: ypspur-coordinator stopped.\n" );
return -1;
}
}
Spur_free( );
return 0;
}
int main( int argc, char *argv[] )
{
int coordinate = CS_FS;
char ip[64];
int port = 0;
int active = 1;
int err = 0;
double vel = 0;
double angvel = 0;
double accel = 0;
double angaccel = 0;
int set_vel = 0;
int set_accel = 0;
int set_angvel = 0;
int set_angaccel = 0;
int msqid = 0;
struct option options[9] =
{
{"set-vel", 1, 0, 'V'},
{"set-angvel", 1, 0, 'W'},
{"set-accel", 1, 0, 'A'},
{"set-angaccel", 1, 0, 'O'},
{"command", 1, 0, 'c'},
{"msq-id", 1, 0, 'q'},
{"socket", 1, 0, 's'},
{"help", 0, 0, 'h'},
{0, 0, 0, 0}
};
int opt;
hook_pre_global();
while( ( opt = getopt_long( argc, argv, "V:W:A:O:c:q:s:h", options, NULL ) ) != -1 )
{
switch ( opt )
{
case 'V':
vel = atof( optarg );
set_vel = 1;
break;
case 'W':
angvel = atof( optarg );
set_angvel = 1;
break;
case 'A':
accel = atof( optarg );
set_accel = 1;
break;
case 'O':
angaccel = atof( optarg );
set_angaccel = 1;
break;
case 'c':
if( msqid == -1 ) YPSpur_init_socket( ip, port );
else if( msqid == 0 ) YPSpur_init( );
else YPSpur_initex( msqid );
proc_spur( optarg, &coordinate );
return 1;
break;
case 'q':
msqid = atoi( optarg );
break;
case 's':
strncpy( ip, optarg, 64 );
{
char *p;
p = strchr( ip, ':' );
if( p == NULL )
{
fprintf( stderr, "USAGE: %s -s ip:port\n", argv[0] );
return -1;
}
*p = 0;
port = atoi( p + 1 );
msqid = -1;
}
break;
case 'h':
print_help( argv );
return 1;
break;
default:
break;
}
}
#if HAVE_SIGLONGJMP
signal( SIGINT, ctrlc );
#endif
if( msqid == -1 ) YPSpur_init_socket( ip, port );
else if( msqid == 0 ) YPSpur_init( );
else YPSpur_initex( msqid );
if( set_vel ) YPSpur_set_vel( vel );
if( set_angvel ) YPSpur_set_angvel( angvel );
if( set_accel ) YPSpur_set_accel( accel );
if( set_angaccel ) YPSpur_set_angaccel( angaccel );
#if HAVE_LIBREADLINE
using_history( );
read_history( ".spurip_history" );
#endif
while( active )
{
static char *line = NULL;
static char *line_prev = NULL;
char text[16];
#if !HAVE_LIBREADLINE
# if HAVE_GETLINE
size_t len;
# endif
#endif
#if HAVE_SIGLONGJMP
if( sigsetjmp( ctrlc_capture, 1 ) != 0 )
{
#if HAVE_LIBREADLINE
write_history( ".spurip_history" );
#endif
}
else
#endif
{
{
// Dummy for error checking
double x, y, th;
YPSpur_get_pos( CS_GL, &x, &y, &th );
}
if( YP_get_error_state() )
{
if( msqid == -1 ) YPSpur_init_socket( ip, port );
else if( msqid == 0 ) YPSpur_init( );
else YPSpur_initex( msqid );
if( set_vel ) YPSpur_set_vel( vel );
if( set_angvel ) YPSpur_set_angvel( angvel );
if( set_accel ) YPSpur_set_accel( accel );
if( set_angaccel ) YPSpur_set_angaccel( angaccel );
if( err == 0 )
{
fprintf( stderr, "WARN: YPSpur-coordinator terminated.\n" );
fflush( stderr );
#if HAVE_SIGLONGJMP
signal( SIGINT, NULL );
#endif
}
err = 1;
yp_usleep( 50000 );
continue;
}
else
{
if( err == 1 )
{
fprintf( stderr, "INFO: YPSpur-coordinator started.\n" );
fflush( stderr );
#if HAVE_SIGLONGJMP
signal( SIGINT, ctrlc );
#endif
}
}
err = 0;
sprintf( text, "%s> ", YPSpur_CSName[coordinate] );
#if HAVE_LIBREADLINE
line_prev = line;
line = readline( text );
if( !line )
{
// EOF
break;
}
if( strlen( line ) > 0 )
{
if( line && line_prev )
{
if( strcmp( line, line_prev ) != 0 )
{
add_history( line );
}
}
else
{
add_history( line );
}
}
#else
printf( "%s", text );
fflush( stdout );
line = NULL;
# if HAVE_GETLINE
len = 0;
getline( &line, &len, stdin );
if( len == 0 ) continue;
# else
line = malloc( 512 );
fgets( line, 512, stdin );
# endif
line_prev = line;
#endif
if( proc_spur( line, &coordinate ) < 0 )
{
active = 0;
}
if( line_prev ) free( line_prev );
}
}
printf( "\n" );
#if HAVE_LIBREADLINE
write_history( ".spurip_history" );
#endif
return 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", ¤t, &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 );
}
