レーザスキャナのシミュレーション
北陽電機のスキャナ式レンジセンサ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;
}
コメント