laservisualbarcode.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      [e-mail]    [e-mail]
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
///////////////////////////////////////////////////////////////////////////
//
// Desc: Driver for detecting combined laser/visual barcodes.
// Author: Andrew Howard
// Date: 17 Aug 2002
// CVS: $Id: laservisualbarcode.cc,v 1.11.2.1 2006/06/07 16:12:48 gerkey Exp $
//
// Theory of operation:
//   Parses a laser scan to find the retro-reflective patches (lines or
//   circles), then points the camera at the patch, zooms in, and
//   attempts to read the colored barcode.  Will not return sensible
//   orientations for circular patches.
//
// Requires:
//   Laser, PTZ and blobfinder devices.
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_laservisualbarcode laservisualbarcode
 * @brief Color laser/visual barcode detector

@todo This driver has not been tested with the v2.0 API

The laser visual barcode detector uses both searches for fiducials that
are both retro-reflective and color-coded.  Fiducials can be either planar
or cylindical, as shown below.  For planar targets, the range, bearing,
orientation and identity will be determined; for cylindrical targets,
the orientation will be undefined.  The target size and shape can be
set in the configuration file.

The laser visual barcode detector searches the laser range data to
find retro-reflective targets, points the camera at each of these
targets in turn, then uses color information to determine the presence
and identity of fiducials.  Thus, this detector makes use of three
underlying devices: a laser range finder, a pan-tilt-zoom camera and a
color blob detector.  Note that the laser is used to determine the
geometry of the fidicual (range, bearing and orientation), while the
camera is used to determine its identity.

The range at which fiducials can be both detected and identified
depends on a number of factors, including the size of the fiducial and
the angular resolution of the laser.  Generally speaking, however,
this detector has better range than the @ref driver_laserbarcode
detector, but produces fewer observations.

See also the @ref driver_laserbar and @ref
driver_laserbarcode drivers.

@image html laservisualbeacon.jpg "A sample laser visual barcode."

@par Compile-time dependencies

- none

@par Provides

- This driver provides detected target information through a @ref
  interface_fiducial device.

@par Requires

- @ref interface_laser
- @ref interface_ptz
- @ref interface_blobfinder

@par Configuration requests

- PLAYER_FIDUCIAL_GET_GEOM

@par Configuration file options

- max_ptz_attention (float)
  - Default: 2.0
  - ??
- retire_time (float)
  - Default: 1.0
  - ??
- max_dist (float) (should be a length?)
  - Default: 0.2
  - ??
- bit_count (integer)
  - Default: 3
  - Number of bits in visual barcode
- bit_width (length)
  - Default: 0.08 m
  - Width of each bit in visual barcode
- bit_height (length)
  - Default: 0.02 m
  - Height of each bit in visual barcode

@par Example

@verbatim
driver
(
  name "laservisualbarcode"
  requires ["laser:0" "ptz:0" "blobfinder:0"]
  provides ["fiducial:0"]
)
@endverbatim

@author Andrew Howard
*/
/** @} */

#include "player.h"

#include <errno.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>       // for atoi(3)
#include <netinet/in.h>   // for htons(3)
#include <unistd.h>

#include <libplayercore/playercore.h>

// Driver for detecting laser retro-reflectors.
class LaserVisualBarcode : public Driver
{
  // Constructor
  public: LaserVisualBarcode( ConfigFile* cf, int section);

  // Setup/shutdown routines.
  public: virtual int Setup();
  public: virtual int Shutdown();

  // Process incoming messages from clients 
  int ProcessMessage (MessageQueue * resp_queue, player_msghdr * hdr, void * data);

  // Info on potential fiducials.
  private: struct fiducial_t
  {
    // Id (-1) if undetermined.
    int id;

    // Pose of fiducial.
    double pose[3];

    // Uncertainty in pose.
    double upose[3];

    // Time at which fiducial was last seen by the laser.
    double laser_time;

    // Time at which ptz selected this fiducial,
    // and the time at which the ptz locked on to this fiducial.
    double ptz_select_time;
    double ptz_lockon_time;

    // Time at which the fiducial was identified.
    double id_time;
  };

  // Info on valid blobs.
  private: struct blob_t
  {
    // Blob channel.
    int ch;
  
    // Blob position in image.
    int x, y;
  };

  // Process laser data.
  // Returns non-zero if the laser data has been updated.
  private: int UpdateLaser(player_laser_data_t * data, double timestamp);

  // Analyze the laser data to find fidicuials (reflectors).
  private: void FindLaserFiducials(double time, player_laser_data_t *data);
    
  // Find the line of best fit for the given segment of the laser
  // scan.  Fills in the pose of the reflector relative to the laser.
  private: void FitLaserFiducial(player_laser_data_t *data, int first, int last, double pose[3]);

  // Match a new laser fiducial against the ones we are already
  // tracking.  The pose is relative to the laser.
  private: void MatchLaserFiducial(double time, double pose[3]);

  // Retire fiducials we havent seen for a while.
  private: void RetireLaserFiducials(double time, player_laser_data_t *data);

  // Update the PTZ to point at one of the laser reflectors.
  private: int UpdatePtz(player_ptz_data_t * data, double timestamp);

  // Select a target fiducial for the PTZ to inspect.
  private: void SelectPtzTarget(double time, player_ptz_data_t *data);

  // Servo the PTZ to a target fiducial.
  private: void ServoPtz(double time, player_ptz_data_t *data);

  // Process any new blobfinder data.
  private: int UpdateBlobfinder(player_blobfinder_data_t * data, double timestamp);

  // Find blobs with valid properties.
  private: void FindBlobs(double time, player_blobfinder_data_t *data);

  // Search the list of valid blobs to locate the visual fiducials.
  private: int FindVisualFiducials(double time, player_blobfinder_data_t *data,
                                   int depth, blob_t *prevblob);
    
  // Update the device data (the data going back to the client).
  private: void UpdateData();
  
  // Fiducial properties.
  private: int barcount;
  private: double barwidth, barheight;

  // Max time to spend looking at a fiducial.
  private: double max_ptz_attention;

  // Retirement age for fiducials that havent been seen for a while.
  private: double retire_time;

  // Max distance between fiducials in successive laser scans.
  private: double max_dist;

  // Laser stuff.
  private: Device *laser;
  private: player_devaddr_t laser_id;
  private: double laser_time;

  // PTZ stuff
  private: Device *ptz;
  private: player_devaddr_t ptz_id;
  private: double ptz_time;

  // Blobfinder stuff.
  private: Device *blobfinder;
  private: player_devaddr_t blobfinder_id;
  private: double blobfinder_time;

  // List of currently tracked fiducials.
  private: int fiducial_count;
  private: fiducial_t fiducials[256];

  // The current selected fiducial for the ptz, the time at which we
  // selected it, and the time at which we first locked on to it.
  private: fiducial_t *ptz_fiducial;

  // Dimensions of the zoomed image for the target fiducial (m).
  private: double zoomwidth, zoomheight;

  // List of current valid blobs.
  private: int blob_count;
  private: blob_t blobs[256];
  
  // Local copy of the current fiducial data.
  private: player_fiducial_data_t fdata;
};


// Initialization function
Driver* LaserVisualBarcode_Init( ConfigFile* cf, int section)
{
  return ((Driver*) (new LaserVisualBarcode( cf, section)));
}


// a driver registration function
void LaserVisualBarcode_Register(DriverTable* table)
{
  table->AddDriver("laservisualbarcode", LaserVisualBarcode_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
LaserVisualBarcode::LaserVisualBarcode( ConfigFile* cf, int section)
  : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_FIDUCIAL_CODE)
{
  // Must have an input laser
  if (cf->ReadDeviceAddr(&this->laser_id, section, "requires",
                       PLAYER_LASER_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);    
    return;
  }
  this->laser = NULL;
  this->laser_time = 0;

  // Must have a ptz
  if (cf->ReadDeviceAddr(&this->ptz_id, section, "requires",
                       PLAYER_PTZ_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);    
    return;
  }
  this->ptz = NULL;
  this->ptz_time = 0;

  // Must have a blobfinder
  if (cf->ReadDeviceAddr(&this->blobfinder_id, section, "requires",
                       PLAYER_BLOBFINDER_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);    
    return;
  }
  this->blobfinder = NULL;
  this->blobfinder_time = 0;

  this->max_ptz_attention = cf->ReadFloat(section, "max_ptz_attention", 2.0);
  this->retire_time = cf->ReadFloat(section, "retire_time", 1.0);
  this->max_dist = cf->ReadFloat(section, "max_dist", 0.2);
  
  // Default fiducial properties.
  this->barcount = cf->ReadInt(section, "bit_count", 3);
  this->barwidth = cf->ReadLength(section, "bit_width", 0.08);
  this->barheight = cf->ReadLength(section, "bit_height", 0.02);

  // Reset fiducial list.
  this->fiducial_count = 0;

  // Reset PTZ target.
  this->ptz_fiducial = NULL;

  // Reset blob list.
  this->blob_count = 0;

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int LaserVisualBarcode::Setup()
{
  
  // Subscribe to the laser.
  if (!(laser = deviceTable->GetDevice (laser_id)))
  {
    PLAYER_ERROR ("unable to locate suitable laser device");
    return -1;
  }
  if (laser->Subscribe (InQueue) != 0)
  {
    PLAYER_ERROR ("unable to subscribe to laser device");
    return -1;
  }

  // Subscribe to the ptz.
  if (!(ptz = deviceTable->GetDevice (ptz_id)))
  {
    PLAYER_ERROR ("unable to locate suitable ptz device");
    return -1;
  }
  if (ptz->Subscribe (InQueue) != 0)
  {
    PLAYER_ERROR ("unable to subscribe to ptz device");
    return -1;
  }

  // Subscribe to the blobfinder.
  if (!(blobfinder = deviceTable->GetDevice (blobfinder_id)))
  {
    PLAYER_ERROR ("unable to locate suitable blobfinder device");
    return -1;
  }
  if (blobfinder->Subscribe (InQueue) != 0)
  {
    PLAYER_ERROR ("unable to subscribe to blobfinder device");
    return -1;
  }

  // Reset blob list.
  this->blob_count = 0;

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int LaserVisualBarcode::Shutdown()
{
  // Unsubscribe from devices.
  laser->Unsubscribe(InQueue);
  ptz->Unsubscribe(InQueue);
  blobfinder->Unsubscribe(InQueue);

  return 0;
}



////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int LaserVisualBarcode::ProcessMessage (MessageQueue * resp_queue, player_msghdr * hdr, void * data)
{
  assert(hdr);
  assert(data);
  
  if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_LASER_DATA_SCAN, laser_id))
  {
    assert(hdr->size == sizeof(player_laser_data_t));
    player_laser_data_t * l_data = reinterpret_cast<player_laser_data_t * > (data);

    UpdateLaser(l_data, hdr->timestamp);

    return 0;
  }

  if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_PTZ_DATA_STATE, ptz_id))
  {
    assert(hdr->size == sizeof(player_ptz_data_t));
    UpdatePtz(reinterpret_cast<player_ptz_data_t * > (data), hdr->timestamp);
    return 0;
  }

  if(Message::MatchMessage (hdr, PLAYER_MSGTYPE_DATA, PLAYER_BLOBFINDER_DATA_BLOBS, blobfinder_id))
  {
    assert(hdr->size == sizeof(player_blobfinder_data_t));
    UpdateBlobfinder(reinterpret_cast<player_blobfinder_data_t * > (data), hdr->timestamp);
    return 0;
  }

/* 
  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_FIDUCIAL_GET_GEOM, device_id))
  {
    int ret = laser->ProcessMessage(PLAYER_MSGTYPE_REQ, PLAYER_LASER_GET_GEOM, laser_id, 0, resp_data, resp_data, resp_len);
    if (ret != PLAYER_MSGTYPE_RESP_ACK)
    	return ret;
  	assert(*resp_len == sizeof(player_laser_geom_t));
  	player_laser_geom_t lgeom = * reinterpret_cast<player_laser_geom_t * > (resp_data);
  	player_fiducial_geom_t * fgeom = reinterpret_cast<player_fiducial_geom_t * > (resp_data);

    fgeom->pose[0] = lgeom.pose[0];
    fgeom->pose[1] = lgeom.pose[1];
    fgeom->pose[2] = lgeom.pose[2];
    fgeom->size[0] = lgeom.size[0];
    fgeom->size[1] = lgeom.size[1];
    fgeom->fiducial_size[0] = ntohs((int) (this->barwidth * 1000));
    fgeom->fiducial_size[1] = ntohs((int) (this->barwidth * 1000));
  
  	*resp_len=sizeof(player_fiducial_geom_t);
  
    return ret;
  }*/
  return -1;
}


////////////////////////////////////////////////////////////////////////////////
// Process laser data.
int LaserVisualBarcode::UpdateLaser(player_laser_data_t * data, double timestamp)
{
  this->laser_time = timestamp;
  
  // Find possible fiducials in this scan.
  this->FindLaserFiducials(timestamp, data);

  // Retire fiducials we havent seen for a while.
  this->RetireLaserFiducials(timestamp, data);

  return 1;
}


////////////////////////////////////////////////////////////////////////////////
// Analyze the laser data to find fidicuials (reflectors).
void LaserVisualBarcode::FindLaserFiducials(double time, player_laser_data_t *data)
{
  unsigned int i;
  int h;
  int valid;
  double r, b;
  double db, dr;
  double mn, mr, mb, mrr, mbb;
  double pose[3];

  // Empty the fiducial list.
  this->fdata.fiducials_count = 0;
  
  // Initialise patch statistics.
  mn = 0.0;
  mr = 0.0;
  mb = 0.0;