Ejemplo n.º 1
0
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;
}
NishidalabYpspurDriver::NishidalabYpspurDriver() : 
  linear_vel_max(1.1),
  angular_vel_max(M_PI),
  linear_acc_max(1.0),
  angular_acc_max(M_PI)
{
  // init yp-spur---------------------------------------------------------------------
  if( Spur_init() < 0)
    ROS_ERROR("can't open ypspur");

  // using parameter server
  n.param("nishidalab_ypspur/linear_vel_max", linear_vel_max, linear_vel_max);
  n.param("nishidalab_ypspur/angular_vel_max", angular_vel_max, angular_vel_max);
  n.param("nishidalab_ypspur/linear_acc_max", linear_acc_max, linear_acc_max);
  n.param("nishidalab_ypspur/angular_acc_max", angular_acc_max, angular_acc_max);
  // init velocity & accelaration limits (Unit is m/s & rad/s)
  Spur_set_vel(linear_vel_max);
  Spur_set_accel(linear_acc_max);
  Spur_set_angvel(angular_vel_max);
  Spur_set_angaccel(angular_acc_max);
  // end init yp-spur------------------------------------------------------------------

  cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("cmd_vel", 10, &NishidalabYpspurDriver::Callback, this);

}
Ejemplo n.º 3
0
int main(void)
{
  if (!Spur_init()) {
    fprintf(stderr, "ERROR : cannot open spur\n");
    return -1;
  }

  // Setting up signal to stops when entering Ctrl-C.
  setSigInt();

  // Setting up velocity.
  Spur_set_vel(0.3);

  // Setting up acceleration.
  Spur_set_accel(1.0);

  // Setting up angular velocity.
  Spur_set_angvel(1.5);

  // Setting up angular acceleration.
  Spur_set_angaccel(2.0);

  // Setting up starting point.
  Spur_set_pos_GL(0, 0, 0);
  

  // <Write here>

  // Stop!
  Spur_stop();

  return 0;
}
NishidalabYpspurOdomPublisher::NishidalabYpspurOdomPublisher() :
  x(0.0),
  y(0.0),
  th(0.0),
  vx(0.0),
  vy(0.0),
  vth(0.0)
{
  odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);

  current_time = ros::Time::now();
  
  if(Spur_init() < 0)
    ROS_ERROR("Can't open spur");
}
Ejemplo n.º 5
0
int main(int argc, char *argv[]) {
    SSMLog<Spur_Odometry>	ssmlog_odometry;	// gyro_odm
    SSMLog<Spur_Odometry>	ssmlog_adjust;		// spur_adjust
    SSMLog<PWSMotor>		ssmlog_mtr;		// moter ssm
    SSMLog<YP_ad>			ssmlog_ad;			// ad
    SSMLog_OdometryErr		ssmlog_odmerr;		// odometry error ratio
    struct {
        double radius_r;
        double radius_l;
        double tread;
        double gear;
        double count_rev;
    } odm_prop;
    Odometer::proc_configuration pconf;		// process configuration
    Odometer::proc_option opt(&pconf);		// process option

    FILE* fp = 0;
    gnd::cui_reader cuireader;


    {   // ---> initialize
        uint32_t phase = 1;

        // get option
        if( opt.get_option(argc, argv) ) {
            return 0;
        }

        ::fprintf(stderr, "========== Initialize ==========\n");
        ::fprintf(stderr, " %d. allocate signal \"\x1b[4mSIGINT\x1b[0m\" to shut-off\n", phase++);
        ::fprintf(stderr, " %d. get kinematics parameter\n", phase++);
        ::fprintf(stderr, " %d. initialize \x1b[4mssm\x1b[0m\n", phase++);
        ::fprintf(stderr, " %d. create ssm-data \"\x1b[4m%s\x1b[0m\"\n", phase++, pconf.output_ssmname.value );
        if( pconf.spur_adjust.value && ::strcmp(pconf.output_ssmname.value, SNAME_ADJUST) ) {
            ::fprintf(stderr, " %d. create ssm-data \"\x1b[4m%s\x1b[0m\"\n", phase++, SNAME_ADJUST );
        }
        if( pconf.gyrodometry.value ) {
            ::fprintf(stderr, " %d. open ssm-data \"\x1b[4m%s\x1b[0m\"\n", phase++, pconf.ad_ssmname.value );
            ::fprintf(stderr, " %d. open ssm-data \"\x1b[4m%s\x1b[0m\"\n", phase++, pconf.motor_ssmname.value );
        }
        if(pconf.debug.value)
            ::fprintf(stderr, " %d. open debug log\n", phase++);
        ::fprintf(stderr, "\n");

        {   // ---> allocate SIGINT to shut-off
            ::proc_shutoff_clear();
            ::proc_shutoff_alloc_signal(SIGINT);
        } // <--- allocate SIGINT to shut-off



        if( !::is_proc_shutoff() ) { // ---> set initial kinematics
            ::fprintf(stderr, "\n");
            ::fprintf(stderr, " => get initial kinematics parameter\n");

            {   // ---> set-zero kinematics property
                odm_prop.radius_l = 0;
                odm_prop.radius_r = 0;
                odm_prop.tread = 0;
                odm_prop.gear = 0;
                odm_prop.count_rev = 0;
            } // <--- set-zero kinematics property

            // read kinematics configure file
            if(*pconf.kfile.value != '\0') {
                ::fprintf(stderr, "    load kinematics parameter file \"\x1b[4m%s\x1b[0m\"\n", pconf.kfile.value);
                read_kinematics_config(&odm_prop.radius_r, &odm_prop.radius_l, &odm_prop.tread,
                                       &odm_prop.gear, &odm_prop.count_rev, pconf.kfile.value);
            }

            // check kinematic parameter
            if( odm_prop.radius_r > 0 && odm_prop.radius_l > 0 && odm_prop.tread > 0 && odm_prop.gear > 0 && odm_prop.count_rev > 0) {
                ::fprintf(stderr, "    load lacking parameter from \x1b[4mypsypur-coordinator\x1b[0m\n");
            }
            else {
                // initialize for using ypspur-coordinator
                if( Spur_init() < 0) {
                    ::fprintf(stderr, "  ... \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to connect ypspur-coordinator.\n");
                    ::proc_shutoff();
                }

                // get kinmeatics from ypspur-coordinator
                if( !::is_proc_shutoff() && YP_get_parameter( YP_PARAM_RADIUS_R, &odm_prop.radius_r ) < 0 ) {
                    ::proc_shutoff();
                }
                if( !::is_proc_shutoff() && YP_get_parameter( YP_PARAM_RADIUS_L, &odm_prop.radius_l ) < 0 ) {
                    ::proc_shutoff();
                }
                if( !::is_proc_shutoff() && YP_get_parameter( YP_PARAM_TREAD, &odm_prop.tread ) < 0 ) {
                    ::proc_shutoff();
                }

                if( !::is_proc_shutoff() && YP_get_parameter(YP_PARAM_COUNT_REV, &odm_prop.count_rev) < 0 ) {
                    ::proc_shutoff();
                }
                if( !::is_proc_shutoff() && YP_get_parameter(YP_PARAM_GEAR, &odm_prop.gear) < 0 ) {
                    ::proc_shutoff();
                }
            }
            // <--- get parameter from ypspur-coordinater

            // check kinematic parameter
            if( odm_prop.radius_r <= 0 || odm_prop.radius_l <= 0 || odm_prop.tread <= 0 || odm_prop.gear <= 0 || odm_prop.count_rev <= 0) {
                ::fprintf(stderr, "  => \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m : Incomplete loading kinematics parameter.\n");
                ::proc_shutoff();
            }
        } // <--- set initial kinematics



        // ---> initialize ssm
        if( !::is_proc_shutoff() ) {
            ::fprintf(stderr, "\n");
            ::fprintf(stderr, " => initialize SSM\n");
            if( !initSSM() ) {
                ::fprintf(stderr, "  \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: SSM is not available.\n");
                ::proc_shutoff();
            }
            else {
                ::fprintf(stderr, "   ... \x1b[1mOK\x1b[0m\n");
            }
        } // <--- initialize ssm


        // ---> ssm odometry open
        if( !::is_proc_shutoff() ) {
            ::fprintf(stderr, "\n");
            ::fprintf(stderr, " => create ssm-data \"\x1b[4m%s\x1b[0m\"\n", pconf.output_ssmname.value );
            ssmk_odometry.data.x = 0;
            ssmk_odometry.data.y = 0;
            ssmk_odometry.data.theta = 0;
            ssmk_odometry.data.v = 0;
            ssmk_odometry.data.w = 0;
            if( !ssmk_odometry.create( pconf.output_ssmname.value, pconf.output_ssmid.value, 5, 0.005 ) ) {
                ::fprintf(stderr, "  \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to create \"\x1b[4m%s\x1b[0m\"\n", pconf.output_ssmname.value );
                ::proc_shutoff();
            }
            else {
//				ssmlog_odometry.write();
                ::fprintf(stderr, "  ... \x1b[1mOK\x1b[0m\n");
            }
        } // <--- ssm gyro-odometry open


        if( !::is_proc_shutoff() && pconf.spur_adjust.value && ::strcmp(pconf.output_ssmname.value, SNAME_ADJUST) ) {
            ::fprintf(stderr, "\n");
            ::fprintf(stderr, " => create ssm-data \"\x1b[4m%s\x1b[0m\"\n", SNAME_ADJUST );
            ssmlog_adjust.data.x = 0;
            ssmlog_adjust.data.y = 0;
            ssmlog_adjust.data.theta = 0;
            ssmlog_adjust.data.v = 0;
            ssmlog_adjust.data.w = 0;
            if( !ssmlog_adjust.create( SNAME_ADJUST, 0, 5, 0.005 ) ) {
                ::fprintf(stderr, "  \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to create \"\x1b[4m%s\x1b[0m\"\n", pconf.output_ssmname.value );
                ::proc_shutoff();
            }
            else {
                ssmlog_adjust.write();
                ::fprintf(stderr, "  ... \x1b[1mOK\x1b[0m\n");
            }

        }

        // ---> ssm ad conversion open
        if( !::is_proc_shutoff() && pconf.gyrodometry.value ) {
            ::fprintf(stderr, "\n");
            ::fprintf(stderr, " => open ssm-data \"\x1b[4m%s\x1b[0m\"\n", pconf.ad_ssmname.value );
            if( !ssmlog_ad.openWait(pconf.ad_ssmname.value, pconf.ad_ssmid.value, 0.0, SSM_READ) ) {
                ::fprintf(stderr, "  \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to open \"\x1b[4m%s\x1b[0m\"\n", pconf.ad_ssmname.value );
                ::proc_shutoff();
            }
            else {
                ::fprintf(stderr, "  ... \x1b[1mOK\x1b[0m\n");
            }
        } // <--- ssm ad conversion open


        // ---> ssm ad conversion open
        if( !::is_proc_shutoff() && pconf.odmerr_id.value >= 0 ) {
            ::fprintf(stderr, "\n");
            ::fprintf(stderr, " => open ssm-data \"\x1b[4m%s\x1b[0m\"\n", pconf.odmerr_name.value );
            if( !ssmlog_odmerr.open(pconf.odmerr_name.value) ) {
                ::fprintf(stderr, "  \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to open \"\x1b[4m%s\x1b[0m\"\n", pconf.odmerr_name.value );
                ::proc_shutoff();
            }
            else {
                ::fprintf(stderr, "  ... \x1b[1mOK\x1b[0m\n");
            }
        } // <--- ssm ad conversion open


        // ---> ssm pws-motor open
        if( !::is_proc_shutoff() ) {
            ::fprintf(stderr, "\n");
            ::fprintf(stderr, " => open ssm-log \"\x1b[4m%s\x1b[0m\"\n", pconf.motor_ssmname.value );
            if( !ssmlog_mtr.open( pconf.motor_ssmname.value ) ) {
                ::fprintf(stderr, "  \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to open \"\x1b[4m%s\x1b[0m\"\n", pconf.motor_ssmname.value );
                ::proc_shutoff();
            }
            else {
                ::fprintf(stderr, "  ... \x1b[1mOK\x1b[0m\n");
            }
        } // <--- ssm pws-motor open

        // initialize cui
        cuireader.set_command( Odometer::cui_cmd, sizeof( Odometer::cui_cmd ) / sizeof( Odometer::cui_cmd[0] ));

        if(pconf.debug.value) {
            fp = ::fopen(__Debug_Log__, "w");
            ::fprintf(fp, "#[1.time] [2.x] [3.y] [4.theta] [5.v] [6.w] [7.ad] [8.bias] [9.sf]\n");
        }

    } // <--- initialize


    {   // ---> operation
        bool show_st = true;
        ssmTimeT init_time = 0;
        ssmTimeT prev_time = -1;
        double cuito = 0;
        double bias	= pconf.bias.value;
        double bias_update_ratio = 0.01;

        bool error_generate = false;

        init_time = ssmlog_mtr.time;

        if(pconf.error_simulation.value > 0.0) { // ---> error simulation initialize
            gnd::random_set_seed();
            error_generate = pconf.error_distributuion.value[0] || pconf.error_distributuion.value[1] ||  pconf.error_distributuion.value[2];
        } // <--- error simulation initialize


        // ---> while
        while( !::is_proc_shutoff() ) {
            // ---> read encoder counter
            if( ssmlog_mtr.readNext() ) {
                double v, w;
                double dt;
                double vol;

                if( prev_time < 0 || ssmlog_mtr.time <= prev_time) {
                    prev_time = ssmlog_mtr.time;
                    continue;
                }
                dt = ssmlog_mtr.time - prev_time;

                {   // ---> compute odometry
                    double wr = ( 2.0 * odm_prop.radius_r * M_PI * ( (double) ssmlog_mtr.data.counter2 ) ) / ( odm_prop.count_rev * odm_prop.gear );
                    double wl = ( 2.0 * odm_prop.radius_l * M_PI * ( (double) ssmlog_mtr.data.counter1 ) ) / ( odm_prop.count_rev * odm_prop.gear );
                    v = (wr + wl) / 2 / dt;
                    w = (wr - wl) / odm_prop.tread / dt;
                } // <--- compute odometry


                // ---> compute angular velocity with gyro-sensor
                if( pconf.gyrodometry.value ) {
                    // read A/D
                    ssmlog_ad.readTime( ssmlog_mtr.time );
                    vol = ssmlog_ad.data.ad[pconf.ratio_port.value] * pconf.voltage.value / (1 << pconf.ad_bits.value);
                    // in the case of just about stationary
                    // compute bias
                    if( ::abs(ssmlog_mtr.data.counter1) ==0 && ::abs(ssmlog_mtr.data.counter2) ==0 ) {
                        bias += ( vol - bias ) * bias_update_ratio;
                    }
                    else {
                        w = -gnd_deg2ang( 1.0 / pconf.scale_factor.value ) * (vol - bias);
                    }
                } // <--- compute angular velocity with gyro-sensor


                {   // ---> transration
                    ssmk_odometry.data.v = v;
                    ssmk_odometry.data.w = w;
                    ssmk_odometry.data.x += v * dt * ::cos(ssmk_odometry.data.theta);
                    ssmk_odometry.data.y += v * dt * ::sin(ssmk_odometry.data.theta);
                    ssmk_odometry.data.theta += w * dt;

                    // ---> error simulation
                    if( error_generate && pconf.error_simulation.value * v * dt > gnd::random_uniform() ) {
                        gnd::matrix::fixed<3, 1> ws, error;
                        gnd::matrix::fixed<3, 3> error_distribution;

                        gnd::matrix::set_zero(&error_distribution);
                        error_distribution[0][0] = pconf.error_distributuion.value[0] * pconf.error_distributuion.value[0];
                        error_distribution[1][1] = pconf.error_distributuion.value[1] * pconf.error_distributuion.value[1];
                        error_distribution[2][2] = gnd_deg2ang(pconf.error_distributuion.value[2]) * gnd_deg2ang(pconf.error_distributuion.value[2]);

                        gnd::random_gaussian_mult( &error_distribution, 3, &ws, &error );

                        ssmk_odometry.data.x += v * error[0][0];
                        ssmk_odometry.data.y += v * error[1][0];
                        ssmk_odometry.data.theta += error[2][0];
                        fprintf(fp, "#error %lf %lf %lf %lf\n", ssmk_odometry.time - init_time,
                                error[0][0], error[1][0], error[2][0]);
                    } // <--- error simulation

                    ssmk_odometry.write( ssmlog_mtr.time );
                    prev_time = ssmlog_mtr.time;
                    if( ssmlog_adjust.isOpen() ) {
                        ssmlog_adjust.data = ssmk_odometry.data;
                        ssmlog_adjust.write( ssmlog_mtr.time );
                    }

                    // debug log
                    if(pconf.debug.value && fp) {
                        fprintf(fp, "%lf %lf %lf %lf %lf %lf %d %lf\n", ssmk_odometry.time - init_time,
                                ssmk_odometry.data.x, ssmk_odometry.data.y, ssmk_odometry.data.theta,
                                ssmk_odometry.data.v, ssmk_odometry.data.w,
                                ssmlog_ad.data.ad[pconf.ratio_port.value], bias);
                    }
                } // <--- transration


            } // <--- read encoder counter

            while( !::is_proc_shutoff() && ssmlog_odmerr.readNext() ) { // ---> marge odm error
                ssmk_odometry.data.x -= ssmlog_odmerr.data.dx;
                ssmk_odometry.data.y -= ssmlog_odmerr.data.dy;
                if( !pconf.gyrodometry.value ) {
                    ssmk_odometry.data.theta -=  ssmlog_odmerr.data.dtheta;
                }

                ssmk_odometry.write( ssmlog_mtr.time );
                if( ssmlog_adjust.isOpen() ) {
                    ssmlog_adjust.data = ssmk_odometry.data;
                    ssmlog_adjust.write( ssmlog_mtr.time );
                }

                // debug log
                if(pconf.debug.value && fp) {
                    fprintf(fp, "%lf %lf %lf %lf %lf %lf %d %lf # fix\n", ssmk_odometry.time - init_time,
                            ssmlog_odometry.data.x, ssmlog_odometry.data.y, ssmlog_odometry.data.theta,
                            ssmlog_odometry.data.v, ssmlog_odometry.data.w,
                            ssmlog_ad.data.ad[pconf.ratio_port.value],
                            bias);
                }
            } // ---> marge odm error

            {   // ---> cui
                int cuival = 0;
                char cuiarg[512];

                ::memset(cuiarg, 0, sizeof(cuiarg));
                if( cuireader.poll(&cuival, cuiarg, sizeof(cuiarg), cuito) > 0 ) {
                    if( show_st ) {
                        // if show status mode, quit show status mode
                        show_st = false;
                        ::fprintf(stderr, "-------------------- cui mode --------------------\n");
                    }
                    else {
                        switch(cuival) {
                        // exit
                        case 'Q': ::proc_shutoff();
                            break;
                        // help
                        case 'h':
                            cuireader.show(stderr, "   ");
                            break;
                        // show status
                        case 's':
                            show_st = true;
                            break;
                        // stand-by mode
                        case 'B':
                            cuito = -1;
                            break;
                        // start
                        case 'o':
                            cuito = 0;
                            break;
                        // debug log-mode
                        case 'D': {
                            ::fprintf(stderr, "   => debug-log mode\n");
                            if( ::strncmp("on", cuiarg, 2) == 0) {
                                bool flg = fp ? true : false;

                                if( !fp && !(fp = fopen(__Debug_Log__, "w")) ) {
                                    ::fprintf(stderr, "    ... \x1b[1m\x1b[31mError\x1b[39m\x1b[0m: fail to open \"\x1b[4m%s\x1b[0m\"", __Debug_Log__);
                                    pconf.debug.value = false;
                                }
                                else if( flg ) {
                                    fprintf(fp, "# 1.[time] 2.[x] 3.[y] 4.[theta] 5.[v] 6.[w] 7.[wh mean] 8.[wh ratio] 9.[trd ratio] 10.[pwm1] 11.[pwm2] 12.[counter1] 13.[counter2]\n");
                                    pconf.debug.value = true;
                                    ::fprintf(stderr, "    ... \x1b[1mon\x1b[0m\n");
                                }
                                else {
                                    pconf.debug.value = true;
                                    ::fprintf(stderr, "    ... \x1b[1mon\x1b[0m\n");
                                }
                            }
                            else if( ::strncmp("off", cuiarg, 3) == 0) {
                                pconf.debug.value = false;
                                ::fprintf(stderr, "    ... \x1b[1moff\x1b[0m\n");
                            }
                            else {
                                ::fprintf(stderr, "   ... %s\n", pconf.debug.value ? "on": "off");
                                ::fprintf(stderr, "   if you want to change mode, input \"on/off\"\n");
                            }
                        }
                        break;
                        case '\0':
                            break;
                        default:
                            ::fprintf(stderr, "   ... \x1b[31m\x1b[1mError\x1b[0m\x1b[39m: invalid command\n");
                            ::fprintf(stderr, "       Please input \x1b[4mhelp\x1b[0m/\x1b[4mh\x1b[0m to show command\n");
                            break;
                        }
                    }
                    ::fprintf(stderr, "  \x1b[33m\x1b[1m%s\x1b[0m\x1b[39m > ", Odometer::proc_name);
                    cuireader.poll(&cuival, cuiarg, sizeof( cuiarg ), 0);
                }
            }// ---> cui


            if( show_st ) { // ---> show status
                struct timespec cur;
                static struct timespec next;
                clock_gettime(CLOCK_REALTIME, &cur);

                if( cur.tv_sec > next.tv_sec ||
                        ( cur.tv_sec == next.tv_sec && cur.tv_nsec > next.tv_nsec )) {

                    ::fprintf(stderr, "\x1b[0;0H\x1b[2J");	// display clear
                    ::fprintf(stderr, "-------------------- \x1b[33m\x1b[1m%s\x1b[0m\x1b[39m --------------------\n", Odometer::proc_name);
                    ::fprintf(stderr, "    position : %.02lf[m] %.02lf[m] %.01lf[deg]\n", ssmlog_odometry.data.x, ssmlog_odometry.data.y,  gnd_ang2deg(ssmlog_odometry.data.theta) );
                    ::fprintf(stderr, "    velocity : v %.02lf[m/s]  w %.03lf[deg/s]\n", ssmlog_odometry.data.v, gnd_ang2deg(ssmlog_odometry.data.w) );
                    ::fprintf(stderr, "        mode : %s\n", pconf.gyrodometry.value ? "gyrodometry" : "odometry" );
                    ::fprintf(stderr, "\n");
                    ::fprintf(stderr, " Push \x1b[1mEnter\x1b[0m to change CUI Mode\n");
                    next = cur;
                    next.tv_sec++;
                }
            } // <--- show status

            // stand-by mode
            if( cuito < 0)	continue;

        } // <--- while

    } // <--- operation



    {   // ---> finalize
        ::endSSM();
        ::fprintf(stderr, "\n");
        ::fprintf(stderr, "Finish.\x1b[49m\n");
    } // <--- finalize

    return 0;
}