つくばチャレンジ2010: レーザスキャナのシミュレーション

レーザスキャナのシミュレーション

北陽電機のスキャナ式レンジセンサUTM-30LXのシミュレータを作りました．URGのAPIを実装していないので不完全です．　ODEでのRAYジオメトリの使い方は参考になると思います．

ここからソースコードを取得できます．ほとんどテストしていないので間違いがあるかもしれません．無保証です．自由に使ってください．ここからソースコードlocalization100731.zipをダウンロードできます．Windows用Codeblocksのプロジェクトファイルしかありません．

以下，関係のある部分のソースコードを紹介します．

/*** レーザスキャナの作成 ***/
static void makeLaserScanner()
{
    dMass mass;

    // レーザスキャナ　
    laser.body  = dBodyCreate(world);
    dMassSetZero(&mass);
    dMassSetCylinderTotal(&mass,LASER_M,3,LASER_R, LASER_L);
    dBodySetMass(laser.body,&mass);
    dBodySetPosition(laser.body,START_X, START_Y, START_Z+NECK_L+SENSOR_L+LASER_L/2);

    // レーザビーム．Rayジオメトリで実装．ボディはない．
    for (int i=0; i < BEAM_NUM; i++)
    {
        beam[i] =  dCreateRay(space, 0);
        dGeomRaySetLength(beam[i], BEAM_LENGTH);
    }
}

/*** レーザー光の発射　***/
static void emitLaser()
{
    dMatrix3 R1, R2, R3, R4, R5;

    const dReal *p = (const dReal *) dBodyGetPosition(laser.body);
    dReal heading_angle = -heading();
    dReal pitch  = dJointGetHingeAngle(laser_joint);

    dRFromAxisAndAngle(R1,1,0,0, -M_PI/2);
    dRFromAxisAndAngle(R4,1,0,0, pitch);

    for (int i=urg.first_index; i < urg.last_index; i+=urg.skip_lines)
    {
        for (int j=0; j < 3; j++)
        {
            contact_pos[i][j] = -1;
        }

        dRFromAxisAndAngle(R2,0,0,1,
                           heading_angle +(double) (i - (BEAM_NUM/2))/(BEAM_NUM/2) * LASER_LO_LIMIT * (M_PI/180));
        dMultiply0(R3,R2,R1,3,3,3);
        dMultiply0(R5,R4,R3,3,3,3);
        dGeomSetPosition(beam[i], p[0], p[1], p[2]);
        dGeomSetRotation(beam[i],R5);
    }
}

/*** レーザの描画　***/
void drawLaser()
{
    const dReal *pos;
    dsSetColorAlpha(1.0, 0.0, 0.0, 0.5);
    static int skip = 6; // 描画を高速にするための間引き数

    pos = dBodyGetPosition(laser.body);
    for (int i = 0; i < BEAM_NUM; i++)
    {
        if (!((contact_pos[i][0] == -1) && (contact_pos[i][1] == -1) &&
                (contact_pos[i][2] == -1)))
        {
            // 描画を高速にするために間引く
            if ((i % skip) == 0)dsDrawLine(pos,(const double *) contact_pos[i]);
        }
    }
}

/*** コールバック関数 ***/
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
    int i,n;
    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);

    if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return;

    static const int N = 10;
    dContact contact[N];
    n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
    if (n > 0)
    {
        for (i=0; i<n; i++)
        {
            if ((o1 == wheel[FRONT].geom) || (o2 == wheel[FRONT].geom) ||
                    (o1 == wheel[REAR].geom)  || (o2 == wheel[REAR].geom))
            {
                contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
                //contact[i].surface.mode = dContactSoftERP | dContactSoftCFM ;
                contact[i].surface.mu = 0.0; // dInfinity; // 摩擦係数0
                contact[i].surface.soft_erp = 1.0;       // 地面のERP
                contact[i].surface.soft_cfm = 1e-5;      // 地面のCFM
            }
            else
            {
                // レーザビームの衝突判定
                if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass)
                {
                    for (int k=0; k < BEAM_NUM; k++)
                    {
                        if (o1 == beam[k] || o2 == beam[k])
                        {
                            // 物体をつきなけないために
                            if ((contact_pos[k][0] == -1) && (contact_pos[k][1] == -1)
                                    && (contact_pos[k][2]==-1))
                            {
                                for (int j=0; j < 3; j++)
                                    contact_pos[k][j] = contact[i].geom.pos[j];
                            }
                            else
                            {
                                return;　
                            }
                        }
                    }
                    return;　// 接触点を見つけるだけなので戻る
                }

                contact[i].surface.mode = // dContactSlip1 | dContactSlip2 |
                    dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactMu2;
                contact[i].surface.mu  = 1.0; // dInfinity; // 摩擦係数無限大
                contact[i].surface.slip1    = 0.001;      // 第１摩擦方向の滑り
                contact[i].surface.slip2    = 0.001;      // 第２摩擦方向の滑り
                contact[i].surface.soft_erp = 1.0;       // 地面のERP
                contact[i].surface.soft_cfm = 1e-5;      // 地面のCFM
            }
            dJointID c = dJointCreateContact(world,contactgroup,&contact[i]); //接触ジョイントの生成
            dJointAttach(c,b1,b2);                   // ジョイントの結合
        }
    }
}

/*** レーザのデータ取得　***/
void laser_receiveData(laser_t *urg, long data[], int data_max)
{
    const dReal *pos;

    pos = dBodyGetPosition(laser.body);
    for (int i = 0; i < BEAM_NUM; i++)
    {
        if (!((contact_pos[i][0] == -1) && (contact_pos[i][1] == -1) &&
                (contact_pos[i][2] == -1)))
        {
            // 描画を高速にするために間引く
            dReal d2 = 0;
            for (int j = 0; j < 3; j++)
            {
                d2 += (pos[j]-contact_pos[i][j]) * (pos[j] - contact_pos[i][j]);
            }
            data[i] = (long) (1000 * sqrt(d2));
            // printf("Laser dist[%d]=%.2f \n",i,laser_dist[i]);
        }
        else
        {
            data[i] = -1;
        }
    }
}

void laser_setSkipLines(laser_t *urg, int skip_lines)
{
    urg->skip_lines = skip_lines;
}

void laser_requestData(laser_t *urg, laser_request_type request_type, int first_index, int last_index)
{
    if ((first_index < 0)||(first_index > BEAM_NUM)|| (first_index > last_index))
    {
        printf("first index error \n");
    }
    else
    {
        urg->first_index = first_index;
    }

    if ((last_index < 0) || (last_index > BEAM_NUM) || (last_index < first_index))
    {
        printf("last index error \n");
    }
    else
    {
        urg->last_index  = last_index;
    }
}

/*** シミュレーションループ ***/
static void simLoop(int pause)
{
    double vel = 10.0;
    static double laser_angle = 0;
    static double diff = 1;

    emitLaser(); // レーザー光発射
    if (!pause)
    {
        controlWheel(v_r, v_l);

        if ((laser_angle >= M_PI/4) || (laser_angle <= - M_PI/4))
        {
            diff *= -1;
        }
        swingLaser(laser_angle += 5.0 * diff * M_PI/180.0);

        deadReckoning();
        //navigation(vel);

        dSpaceCollide(space,0,&nearCallback);
        dWorldQuickStep(world, STEP_SIZE);
        dJointGroupEmpty(contactgroup);
    }

    laser_receiveData(&urg, laser_data,laser_data_max);
    //for (int i=0; i < laser_data_max;i++) {
    //    printf("Laser dist[%d]=%d \n",i,laser_data[i]);
    //}

    //drawParticles();
    drawRobot();
    if (show) drawLaser();
    drawWall();
    steps++;
}

/*** レーザスキャナの初期化 ***/
void initLaser()
{
    laser_data = (long*)malloc(sizeof(long) * laser_data_max);
    if (laser_data == NULL)
    {
        perror("malloc");
        exit(1);
    }
    laser_setSkipLines(&urg,2);
    laser_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
}

/*** メイン関数 ***/
int main(int argc, char *argv[])
{
    setDrawStuff(); // 描画関数の設定
    srand(time(NULL));
    dInitODE();
    world        = dWorldCreate();              // ワールドの生成
    space        = dHashSpaceCreate(0);         // スペースの生成
    contactgroup = dJointGroupCreate(0);        // 接触点グループの生成
    ground       = dCreatePlane(space,0,0,1,0); // 地面の生成

    dWorldSetGravity(world, 0, 0, -9.8); // 重力加速度の設定
    //dWorldSetCFM(world,1e-5); // CFMの設定
    dWorldSetERP(world,1.0);  // ERPの設定 0.8

    makeRobot(); // ロボットの生成
    makeWall(); // 壁の生成
    initLaser(); // レーザスキャナの初期化

    dsSimulationLoop(argc,argv,800,480,&fn); // シミュレーションループ

    dJointGroupDestroy(contactgroup);  // 接触点グループの破壊
    dSpaceDestroy(space);              // スペースの破壊
    dWorldDestroy(world);              // ワールドの破壊
    dCloseODE();
    return 0;
}