lasersafe.cc

机器人仿真软件

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_lasersafe lasersafe
 * @brief laser monitor

This is a low level safety 'driver' that temporarily disables
velocity commands if a laser range is detected within a specified
safe distance. It sits on top of @ref interface_laser and
@ref interface_position2d devices.

The general concept of this device is to not do much, but to provide
a last line of defense in the case that higher level drivers or client
software fails in its object avoidance.
When the laser scanner detects an obstacle within the safe distance, it
will prevent the robot from moving either forwards or backwards
(depending on which way the laser scanner is facing). For example, if
the laser scanner is facing forwards and it detects an obstacle, the
robot will only be able to back away, not continue forwards into the
obstacle.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_position2d

@par Requires

- @ref interface_position2d : the underlying robot to be controlled
- @ref interface_laser : the laser to read from

@par Configuration requests

- PLAYER_POSITION2D_MOTOR_POWER_REQ : if motor is switched on then we
  reset the 'safe state' so robot can move with a bump panel active
- all other requests are just passed on to the underlying @ref
  interface_position2d device

@par Configuration file options

- safedistance
  Default: 0.4m
  The distance at which to stop
- step
  Default: 5
  The number of range readings to skip over. For example, the default
  value will check readings 0, 5, 10, and so on. A value of 10 will
  check readings 0, 10, 20, 30, ...
- history
  Default: 3
  The driver will use a moving average of range readings to help
  overcome noise issues. This specifies the number of readings to
  consider in total (so a value of 3 gives the current plus the two
  previous readings).
  If set to 1, only the most recent scan data will be used.
- forward
  Default: 1
  Indicates if the laser scanner is pointing forwards (1) or backwards (0).
- boxmode
  Default: 1
  If 1, the driver uses a box model for the safety area instead of a radius
  distance from the laser scanner. This can allow you to, for example, ensure
  that the robot can pass through narrow passages without driving into an
  object ahead. Set to 0 to use the radius mode.
- boxwidth
  Default: -1m
  The width of the box. If less than zero, the position2d device will be
  queried for the width of the robot and that will be used as the box width.
- boxsafety
  default: 0.1
  A safety margin to use if getting the width of the robot for box mode. Won't
  be used if specifying the width of the box manually in the config file. The
  default of 0.1 is a 10% safety margin, 0.25 would be 25%, and so on.

TODO: Make driver more advanced so that it can find the laser's pose and
allow movement in the opposite direction to the way the laser is pointing.

@par Example

@verbatim
driver
(
  name "lasersafe"
  provides ["position2d:0"]
  requires ["position2d:1" "laser:0"]
  safedistance 0.3
  step 10
  history 1
)
@endverbatim

@author Toby Collett, Geoffrey Biggs

*/
/** @} */

#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <netinet/in.h>
#include <unistd.h>
#include <errno.h>
#include <time.h>
#include <math.h>

#include <libplayercore/playercore.h>
#include <libplayercore/error.h>

class LaserSafe : public Driver
{
  public:
    // Constructor
    LaserSafe (ConfigFile* cf, int section);

    // Destructor
    virtual ~LaserSafe () {};

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

    // Underlying devices
    int SetupPosition ();
    int ShutdownPosition ();

    int SetupLaser ();
    int ShutdownLaser ();

    // Message Handler
    int ProcessMessage (MessageQueue * resp_queue, player_msghdr * hdr, void * data);

  private:

    bool ScanInRange (double scanDistance, double scanAngle);

    // state info
    bool Blocked;
    player_laser_data_t CurrentState;
    player_laser_data_t *history;
//    player_laser_data_t SafeState;

    // Position device info
    Device *position;
    player_devaddr_t position_id;
    int speed,turnrate;
    double position_time;
    bool position_subscribed;

    // Laser device info
    Device *laser;
    player_devaddr_t laser_id;
    double laser_time;
    bool laser_subscribed;

    // State info
    double safeDistance;
    unsigned int step;
    unsigned int historyLength;
    unsigned int currentHistSlot;
    bool front;
    bool boxMode;
    double boxWidth;
    double boxSafety;
    bool needPoseInfo, gotPoseInfo;
};

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

// a driver registration function
void LaserSafe_Register(DriverTable* table)
{
  table->AddDriver ("lasersafe",  LaserSafe_Init);
  return;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int LaserSafe::Setup ()
{
  // Initialise the underlying devices.
  if (SetupPosition () != 0)
  {
    PLAYER_ERROR2 ("Laser safe failed to connect to undelying position2d device %d:%d\n", position_id.interf, position_id.index);
    return -1;
  }
  if (SetupLaser () != 0)
  {
    PLAYER_ERROR2 ("Laser safe failed to connect to undelying laser device %d:%d\n", laser_id.interf, laser_id.index);
    return -1;
  }

  Blocked = true;

  // Create enough laser history
  if (historyLength > 1)
  {
    if (!(history = new player_laser_data_t[historyLength - 1]))
    {
      PLAYER_WARN1 ("Laser safe failed to create history buffer for history length %d, falling back to no history\n", historyLength);
      historyLength = 1;
      history = NULL;
    }
    else
    {
      memset (history, 0, sizeof (player_laser_data_t) * (historyLength - 1));
    }
    currentHistSlot = 0;
  }

  // Send a request for the robot's width from the position driver if need it
  if (needPoseInfo)
  {
    position->PutMsg (InQueue, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, NULL, 0, NULL);
  }

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int LaserSafe::Shutdown ()
{
  // Stop the laser
  ShutdownPosition ();

  // Stop the odom device.
  ShutdownLaser ();

  // Delete the history (if present)
  if (history)
  {
    delete[] history;
    history = NULL;
  }

  gotPoseInfo = false;

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Check if a laser scan distance is within warning distance
bool LaserSafe::ScanInRange (double scanDistance, double scanAngle)
{
  if (boxMode)
  {
    double x, y;

    x = scanDistance * cos (scanAngle);
    y = scanDistance * sin (scanAngle);

    if (x < safeDistance && fabs (y) < boxWidth)    // Box is centered on laser
    {
      return true;
    }
    return false;
  }
  else
  {
    if (scanDistance < safeDistance)
      return true;
    return false;
  }
}

////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int LaserSafe::ProcessMessage (MessageQueue * resp_queue, player_msghdr * hdr, void * data)