laservisualbw.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 reflectors with B&W barcodes
// Author: Andrew Howard
// Date: 9 Jan 2004
// CVS: $Id: laservisualbw.cc,v 1.14.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 B&W barcode.  Will not return sensible
//   orientations for circular patches.
//
// Requires:
//   Laser, PTZ and camera devices.
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_laservisualbw laservisualbw
 * @brief Black & white laser/visual barcode detector
 
@todo This driver has not been tested with the player 2 API.

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 B&W barcode.  Will not return sensible orientations for
circular patches.

@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_camera

@par Configuration requests

- PLAYER_FIDUCIAL_GET_GEOM

@par Configuration file options

- max_ptz_attention (float)
  - Default: 6.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
- guard_min (integer)
  - Default: 4
  - Minimum height of bit (pixels)
- guard_tol (length)
  - Default: 0.2 m
  - Height tolerance of bit (ratio)
- digit_err_first (float)
  - Default: 0.5
  - Error threshold on the best bit
- digit_err_second (float)
  - Default: 1.0
  - Error threshold on the second-best bit

@par Example

@verbatim
driver
(
  name "laserbar"
  requires ["laser:0"]
  provides ["fiducial:0"]
  width 0.2
)
@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 LaserVisualBW : public Driver
{
  // Constructor
  public: LaserVisualBW( ConfigFile* cf, int section);

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

  // 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;
  };

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

  // 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 camera data.
  private: int UpdateCamera(player_camera_data_t * data, double timestamp);

  // Extract a bit string from the image.  
  private: int ExtractSymbols(int x, int symbol_max_count, int symbols[]);

  // Extract a code from a symbol string.
  private: int ExtractCode(int symbol_count, int symbols[]);
  
  // Write the device data (the data going back to the client).
  private: void WriteData();

  // Image processing
  private: double edge_thresh;
  
  // Barcode tolerances
  private: int barcount;
  private: double barwidth;
  private: double guard_min, guard_tol;
  private: double err_first, err_second;

  // 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;

  // Camera stuff
  private: Device *camera;
  private: player_devaddr_t camera_id;
  private: double camera_time;
  private: player_camera_data_t camera_data;

  // 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;
  
  // Local copy of the current fiducial data.
  private: player_fiducial_data_t fdata;
};


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


// a driver registration function
void LaserVisualBW_Register(DriverTable* table)
{
  table->AddDriver("laservisualbw", LaserVisualBW_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
LaserVisualBW::LaserVisualBW( 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 camera
  if (cf->ReadDeviceAddr(&this->camera_id, section, "requires",
                       PLAYER_CAMERA_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);    
    return;
  }
  this->camera = NULL;
  this->camera_time = 0;

  this->max_ptz_attention = cf->ReadFloat(section, "max_ptz_attention", 6.0);
  this->retire_time = cf->ReadFloat(section, "retire_time", 1.0);
  this->max_dist = cf->ReadFloat(section, "max_dist", 0.2);

  // Image processing
  this->edge_thresh = cf->ReadFloat(section, "edge_thresh", 20);
  
  // Default fiducial properties.
  this->barwidth = cf->ReadLength(section, "bit_width", 0.08);
  this->barcount = cf->ReadInt(section, "bit_count", 3);

  // Barcode properties: minimum height (pixels), height tolerance (ratio).
  this->guard_min = cf->ReadInt(section, "guard_min", 4);
  this->guard_tol = cf->ReadLength(section, "guard_tol", 0.20);

  // Error threshold on the first and second best digits
  this->err_first = cf->ReadFloat(section, "digit_err_first", 0.5);
  this->err_second = cf->ReadFloat(section, "digit_err_second", 1.0);

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

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

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int LaserVisualBW::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 (!(camera = deviceTable->GetDevice (camera_id)))
  {
    PLAYER_ERROR ("unable to locate suitable camera device");
    return -1;
  }
  if (camera->Subscribe (InQueue) != 0)
  {
    PLAYER_ERROR ("unable to subscribe to camera device");
    return -1;
  }
  
  return 0;
}


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

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int LaserVisualBW::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_CAMERA_DATA_STATE, camera_id))
  {
    assert(hdr->size == sizeof(player_camera_data_t));
    UpdateCamera(reinterpret_cast<player_camera_data_t * > (data), hdr->timestamp);
    return 0;
  }

/*
 
  if (MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_FIDUCIAL_GET_GEOM, device_id))
  {
    assert(*resp_len>sizeof(player_fiducial_geom_t));
    assert(*resp_len>sizeof(player_laser_geom_t));

    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 LaserVisualBW::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 LaserVisualBW::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;
  mrr = 0.0;
  mbb = 0.0;
    
  // Look for a candidate patch in scan.
  for (i = 0; i < data->ranges_count; i++)
  {
    r = (double) (data->ranges[i]);
    b = (double) (data->min_angle + i * data->resolution);
    h = (int) (data->intensity[i]);

    // If there is a reflection...
    if (h > 0)
    {
      mn += 1;
      mr += r;
      mb += b;
      mrr += r * r;
      mbb += b * b;
    }

    // If there is no reflection and we have a patch...
    else if (mn > 0)
    {
      // Compute the moments of the patch.
      mr /= mn;
      mb /= mn;
      mrr = mrr / mn - mr * mr;
      mbb = mbb / mn - mb * mb;

      // Test moments to see if they are valid.
      valid = 1;
      valid &= (mn >= 1.0);
      dr = this->barwidth / 2;
      db = atan2(this->barwidth / 2, mr);
      valid &= (mrr < (dr * dr));
      valid &= (mbb < (db * db));
      
      if (valid)
      {
        // Do a best fit to determine the pose of the reflector.
        this->FitLaserFiducial(data, i - (int) mn, i - 1, pose);

        // Match this fiducial against the ones we are already tracking.
        this->MatchLaserFiducial(time, pose);
      }
      
      mn = 0.0;
      mr = 0.0;
      mb = 0.0;
      mrr = 0.0;
      mbb = 0.0;
    }
  }
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Find the line of best fit for the given segment of the laser scan.
// Fills in the pose and pose of the reflector relative to the laser.
void LaserVisualBW::FitLaserFiducial(player_laser_data_t *data,
                                          int first, int last, double pose[3])
{