Esempio n. 1
0
int 
main (int argc, char *argv[])
{
  MSG("Starting");
  int i;
  DO(i = 5);
  IVAR(i);
  IVAR(i*i);
  DO(i++);
  IVAR(i);
  DO(++i);
  MSG("Ending");
  return 0;
} // main (int, char *)
//返回预测的位置
// gao mark
MyVector2d RobotTracker::position(double time)
{
    //# Use a closed form prediction based on current velocity for position.
    //ROBOT_FAST_PREDICT = 1 # true
    if (IVAR(ROBOT_FAST_PREDICT))
    {
        if (time > latency)
        {
            //起始时间大于延迟,向后推延迟长度
            Matrix x = predict(latency);
            return MyVector2d(x.e(0,0), x.e(1,0)) +
                    MyVector2d(x.e(3,0), x.e(4,0)) * (time - latency);
        }
        else
        {
            //起始时间比延迟大,则直接取起点参数
            Matrix x = predict(time);

            return MyVector2d(x.e(0,0), x.e(1,0));
        }
    }
    else
    {
        //返回起点参数
        Matrix x = predict(time);
        return MyVector2d(x.e(0,0), x.e(1,0));
    }
}
void RobotTracker::LoadConfig()
{
    CR_SETUP(tracker, ROBOT_PRINT_KALMAN_ERROR, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_FAST_PREDICT, CR_INT);
    CR_SETUP(tracker, ROBOT_USE_AVERAGES_IN_PROPAGATION, CR_INT);
    CR_SETUP(tracker, ROBOT_CONFIDENCE_THRESHOLD, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_POSITION_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_THETA_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_NONE_VELOCITY_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_NONE_ANGVEL_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_DIFF_VELOCITY_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_DIFF_VELOCITY_VARIANCE_PERP, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_DIFF_ANGVEL_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_OMNI_VELOCITY_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_OMNI_ANGVEL_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_STUCK_VARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_VELOCITY_NEXT_STEP_COVARIANCE, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_STUCK_DECAY, CR_DOUBLE);
    CR_SETUP(tracker, ROBOT_STUCK_THRESHOLD, CR_DOUBLE);

    //# Output Kalman Errors
    //ROBOT_PRINT_KALMAN_ERROR = 0 # false
    if (IVAR(ROBOT_PRINT_KALMAN_ERROR))
    {
        prediction_lookahead = LATENCY_DELAY;
    }
}
// return the velocity un-rotate
//以机器人中心为坐标原点,机器人基准坐标系
MyVector2d RobotTracker::velocity_raw(double time)
{
    Matrix x;

    if (IVAR(ROBOT_FAST_PREDICT))
    {
        if (time > latency)
        {
            x = predict(latency);
        }
        else
        {
            x = predict(time);
        }
    }
    else
    {
        x = predict(time);
    }
    double vx = x.e(3,0);
    double vy = x.e(4,0);

    double stuck = x.e(6,0);

    if (stuck > 0.8) return MyVector2d(0.0, 0.0);

    return MyVector2d(vx, vy);
}
Esempio n. 5
0
static void add_section_to_hash( bfd * abfd, asection * s, PTR data ) {
	VALUE sec;
	VALUE * hash = (VALUE *) data;
	if (! abfd || ! s ) {
		return;
	}
	sec = section_new(abfd, s);
	rb_hash_aset( *hash, rb_iv_get(sec, IVAR(SEC_ATTR_NAME)), sec);
}
Esempio n. 6
0
int
main (int argc, char *argv[])
{
    char *x;
    int i;
    DO(i = 2);
    MSG("Starting");
    IVAR(i);
    IVAR(i*2);
    DO(i = i + 7);
    IVAR(i);
    IVAR(i*2);
    DO(x = "hello");
    MSG("Successfully said hello.");
    DO(*x = 'g');
    MSG("Ending");
    return 0;
} // main(int, char **)
Esempio n. 7
0
static VALUE cls_section_contents(VALUE instance) {
	/* lazy-loading of section list */
	VALUE var = rb_iv_get(instance, IVAR(SEC_ATTR_CONTENTS));
	if ( var == Qnil ) {
		unsigned char * buf;
		unsigned int size;
		asection * sec;

		var = Qnil;
		Data_Get_Struct(instance, asection, sec);
		size = bfd_section_size( sec->owner, sec );
		buf = calloc( size, 1 );
		if ( buf && 
		     bfd_get_section_contents(sec->owner, sec, buf, 0, size) ) {
			var = rb_str_new( (const char *) buf, size );
			rb_iv_set(instance, IVAR(SEC_ATTR_CONTENTS), var); 
		}
	}
	return var;
}
Esempio n. 8
0
static void add_symbol_to_hash( bfd * abfd, asymbol * s, PTR data,
			        char is_dynamic ) {
	VALUE sym;
	VALUE * hash = (VALUE *) data;

	if (! abfd || ! s ) {
		return;
	}

	sym = symbol_new(abfd, s, is_dynamic);
	rb_hash_aset( *hash, rb_iv_get(sym, IVAR(SYM_ATTR_NAME)), sym);
}
Esempio n. 9
0
static VALUE symbol_new(bfd * abfd, asymbol *s, char is_dynamic) {
	symbol_info info;
	VALUE class, instance;
	VALUE argv[1] = { Qnil };

	class = rb_class_new(clsSymbol);
	instance = Data_Wrap_Struct(class, NULL, NULL, s);
	rb_obj_call_init(instance, 0, argv);

	bfd_symbol_info(s, &info);

	/* set instance variables */
	rb_iv_set(instance, IVAR(SYM_ATTR_NAME), rb_str_new_cstr(info.name) );
	rb_iv_set(instance, IVAR(SYM_ATTR_TYPE), INT2NUM((int) info.type) );
	rb_iv_set(instance, IVAR(SYM_ATTR_VALUE), SIZET2NUM(info.value) );
	rb_iv_set(instance, IVAR(SYM_ATTR_FLAGS), INT2NUM(s->flags) );
	rb_iv_set(instance, IVAR(SYM_ATTR_BIND), 
		  rb_str_new_cstr( (is_dynamic ? SYM_BIND_DYNAMIC : 
				  		 SYM_BIND_STATIC) ) );
	if ( s->section ) {
		rb_iv_set(instance, IVAR(SYM_ATTR_SECTION), 
			  rb_str_new_cstr(s->section->name)); 
	}

	return instance;
}
Esempio n. 10
0
static VALUE section_new(bfd * abfd, asection *s) {
	VALUE class, instance;
	VALUE argv[1] = { Qnil };

	class = rb_class_new(clsSection);
	instance = Data_Wrap_Struct(class, NULL, NULL, s);
	rb_obj_call_init(instance, 0, argv);

	/* set instance variables */
	rb_iv_set(instance, IVAR(SEC_ATTR_ID), INT2NUM(s->id) );
	rb_iv_set(instance, IVAR(SEC_ATTR_NAME), rb_str_new_cstr(s->name) );
	rb_iv_set(instance, IVAR(SEC_ATTR_INDEX), INT2NUM(s->index) );
	rb_iv_set(instance, IVAR(SEC_ATTR_FLAGS), INT2NUM(s->flags) );
	rb_iv_set(instance, IVAR(SEC_ATTR_VMA), SIZET2NUM(s->vma) );
	rb_iv_set(instance, IVAR(SEC_ATTR_LMA), SIZET2NUM(s->lma) );
	rb_iv_set(instance, IVAR(SEC_ATTR_SIZE), 
		  SIZET2NUM(bfd_section_size(abfd, s)) );
	rb_iv_set(instance, IVAR(SEC_ATTR_ALIGN), INT2NUM(s->alignment_power) );
	rb_iv_set(instance, IVAR(SEC_ATTR_FPOS), SIZET2NUM(s->filepos) );
	rb_iv_set(instance, IVAR(SEC_ATTR_CONTENTS), Qnil); 

	return instance;
}
//返回预测的速度
//速度以场地坐标系为基准
MyVector2d RobotTracker::velocity(double time)
{
    Matrix x;

    if (IVAR(ROBOT_FAST_PREDICT))
    {
        if (time > latency)
        {
            x = predict(latency);
        }
        else
        {
            x = predict(time);
        }
    }
    else
    {
        x = predict(time);
    }

    double a = x.e(2,0);
    double c = cos(a);
    double s = sin(a);

    double vx = x.e(3,0);
    double vy = x.e(4,0);

    double stuck = x.e(6,0);

    // Threshold after which the robot is considered stuck and zero's velocity.
    //ROBOT_STUCK_THRESHOLD = 0.6 # Set to 1.1 to turn off.
    if (stuck > DVAR(ROBOT_STUCK_THRESHOLD))
    {
        return MyVector2d(0.0, 0.0);
    }
    //fprintf(stderr, "=============>\nx =\n");
    //x.print();
    //fprintf(stderr, "---->\nx =\n");

    MyVector2d v0;
    v0.set(c * vx - s * vy, s * vx + c * vy);
    //旋转角度为负
    return v0;//MyVector2d(c * vx - s * vy, s * vx + c * vy);

}
//Matrix x: [0,0]=x [0,1]=y [0,2]=angle [0,3]=vx [0,4]=vy [0,5]=vangle [0,6]
//计算下一步的x,y,角度
Matrix& RobotTracker::f(const Matrix &x, Matrix &I)
{
    I = Matrix();

    static Matrix f;
    f = x;

    rcommand c = get_command(stepped_time);

    double
            &_x = f.e(0,0),
            &_y = f.e(1,0),
            &_theta = f.e(2,0),
            &_vpar = f.e(3,0),
            &_vperp = f.e(4,0),
            &_vtheta = f.e(5,0),
            &_stuck = f.e(6,0);

    _stuck = bound(_stuck, 0, 1) * DVAR(ROBOT_STUCK_DECAY);

    double avg_vpar = 0, avg_vperp = 0, avg_vtheta = 0, avg_theta = 0;
    double avg_weight = 0.5;

    //# Maybe an improved method for propagating.
    //ROBOT_USE_AVERAGES_IN_PROPAGATION = 0 # false
    if (IVAR(ROBOT_USE_AVERAGES_IN_PROPAGATION)) {
        avg_vpar = avg_weight * _vpar;
        avg_vperp = avg_weight * _vperp;
        avg_vtheta = avg_weight * _vtheta;
    }

    if (type != ROBOT_TYPE_NONE)
    {
        _vpar = c.vs.x;
        _vperp = c.vs.y;
        _vtheta = c.vs.z;
    }

    //# Maybe an improved method for propagating.
    //ROBOT_USE_AVERAGES_IN_PROPAGATION = 0 # false
    if (IVAR(ROBOT_USE_AVERAGES_IN_PROPAGATION))
    {
        avg_vpar += (1.0 - avg_weight) * _vpar;
        avg_vperp += (1.0 - avg_weight) * _vperp;
        avg_vtheta += (1.0 - avg_weight) * _vtheta;

        avg_theta = avg_weight * _theta;

        _theta += (1.0 - _stuck) * stepsize_ * avg_vtheta;
        avg_theta += (1.0 - avg_weight) * _theta;

        _x += (1.0 - _stuck) * stepsize_ *
                (avg_vpar * cos(avg_theta) + avg_vperp * -sin(avg_theta));
        _y += (1.0 - _stuck) * stepsize_ *
                (avg_vpar * sin(avg_theta) + avg_vperp * cos(avg_theta));
    }
    else
    {
        //根据速度更新位置
        _theta += (1.0 - _stuck) * stepsize_ * _vtheta;
        _x += (1.0 - _stuck) * stepsize_ *
                (_vpar * cos(_theta) + _vperp * -sin(_theta));
        _y += (1.0 - _stuck) * stepsize_ *
                (_vpar * sin(_theta) + _vperp * cos(_theta));
    }

    _theta = anglemod(_theta);

    return f;
}