My experience implementing a lidar in torcs (part 1)

Hello,

This is my experience implementing a lidar in torcs, which I hope will
be useful to anyone following a similar path. 

Partly inspired by Brad Templeton's essay
(http://www.templetons.com/brad/robocars/simulator.html) on the need
for an open source driving simulator, I decided to investigate more
about bringing this vision to frutition, as a spare time project.

As most roboticists (e.g. Sebastian Thurn in the udacity course) point
out, the major challenge for a robocar is perception. Once a robocar
knows where the road is, where *it* is located and where the other
cars and obstacles are, it is relatively easy to decide what to do and
signal actuators. This insight is validated by how good the built in
TORCS robots are, which have access to all this information. Almost
all of them manage to soundly kick my ass on every racetrack (I'm
playing with a keyboard, maybe I'd do better if I played with a wheel
or joystick).

Simulated Car Racing (SCR) is a good effort to make the robots more
realistic, but only goes a small distance. The track and opponent
sensors that SCR provides (via UDP) are still a bit too much spoon
feeding. Realistic sensors (such as GPS, Inertial Measurement Unit
(IMU), lidar, radar, stereo cameras) used by real self driving cars
are all missing.

The good news is that most of these sensors are easy to implement. GPS
and IMU are trivial as car position and acceleration are directly
available via the `tCarElt* car` class/struct, viz.:

car->_pos_X
car->_pos_Y
car->_pos_Z
car->_roll
car->_pitch
car->_yaw
car->_accel_x
car->_accel_y
car->_accel_z

All that is needed for more realism is to add Gaussian noise (with
proper sigma) and random dropouts for the GPS and simulated drift for
the IMU (if it is too accurate).

It is also fortunate that a simple lidar, such as a single beam SICK
type is easy to implement in a few lines of code, using plib's ssgLOS
(line of sight) function (http://plib.sourceforge.net/ssg/non_class.html):

#+begin_src cpp
void lidar(const tCarElt *car, sgVec3 pos, sgVec3 l_dir, sgVec4 xyzr)
{
  // pos:   position of the sensor relative to the car->_postMat
  // l_dir: direction in which laser points relative to the sensor
  
  sgMat4 m;      // sensor position matrix
  sgCopyMat4(m, car->_posMat);
  m[3][0] += pos[0]; m[3][1] += pos[1]; m[3][2] += pos[2]; 
  sgMat4 inv_m;  // inverse of sensor position matrix
  sgTransposeNegateMat4(inv_m, m);

  ssgHit *results;
  int num_hits = ssgLOS(TheScene, l_dir, inv_m, &results);

  if (num_hits == 0) {
    xyzr[3] = 0;
    return;
  }

  ssgHit *hit;
  // range initialized to a large number
  float range = 1E9, curr_range;
  for (int i = 0; i < num_hits; ++i) {
    hit = &results[i];
	
	// divide-by-zero check omitted on purpose
    curr_range = hit->plane[3] / sgScalarProductVec3(hit->plane, l_dir);

    if (curr_range < range)
      range = curr_range;
  }

  sgVec3 xyz;
  sgCopyVec3(xyz, l_dir);
  sgScaleVec3(xyz, range);
  sgXformPnt3(xyz, xyz, m);

  xyzr[0] = xyz[0];
  xyzr[1] = xyz[1];
  xyzr[2] = xyz[2];
  xyzr[3] = range; 
}
#+end_src

If there is sufficient interest, I'll soon post a part 2 going into an
explanation for the code and how I plan to extend it to simulate a
more sophisticated multi beam lidar such as the Velodyne HDL-64 (which
was extensively used in Darpa GC 2007 and is the main sensor of the
Google SDC).

regards,
ravich2-7183 

------------------------------------------------------------------------------
Start Your Social Network Today - Download eXo Platform
Build your Enterprise Intranet with eXo Platform Software
Java Based Open Source Intranet - Social, Extensible, Cloud Ready
Get Started Now And Turn Your Intranet Into A Collaboration Platform
http://p.sf.net/sfu/ExoPlatform