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);
}
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);
}
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 **)
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;
}
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);
}
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;
}
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;
}
