wavefront.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2003  Brian Gerkey   [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
 *
 */

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_wavefront wavefront
 * @brief Wavefront-propagation path-planner

The wavefront driver implements a global path planner for a planar
mobile robot.

This driver works in the following way: upon receiving a new @ref
interface_planner target, a path is planned from the robot's
current pose, as reported by the underlying @ref interface_localize
device.  The waypoints in this path are handed down, in sequence,
to the underlying @ref interface_position2d device, which should
be capable of local navigation (the @ref driver_vfh driver is a
great candidate). By tying everything together in this way, this driver
offers the mythical "global goto" for your robot.

The planner first creates a configuration space of grid cells from the
map that is given, treating both occupied and unknown cells as occupied.
The planner assigns a cost to each of the free cells based on their
distance to the nearest obstacle. The nearer the obstacle, the higher
the cost. Beyond the max_radius given by the user, the cost in the
c-space cells is zero.

When the planner is given a new goal, it finds a path by working its
way outwards from the goal cell, assigning plan costs to the cells as
it expands (like a wavefront expanding outwards in water). The plan
cost in each cell is dependant on its distance from the goal, as well
as the obstacle cost assigned in the configuration space step. Once the
plan costs for all the cells have been evaluated, the robot can simply
follow the gradient of each lowest adjacent cell all the way to the goal.

In order to function effectively with an underlying obstacle avoidance
algorithm, such as Vector Field Histogram (the @ref driver_vfh
driver), the planner only hands off waypoints, not the entire path. The
wavefront planner finds the longest straight-line distances that don't
cross obstacles between cells that are on the path. The endpoints of
these straight lines become sequential goal locations for the underlying
device driving the robot.

For help in using this driver, try the @ref util_playernav utility.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_planner

@par Requires

This driver controls two named position2d devices: one for input and one
for output.  That way you can read poses from a localization or SLAM system
and send commands directly to the robot.  The input and output devices may
be the same.

- "input" @ref interface_position2d : source of current pose information
  (usually you would use the @ref driver_amcl driver)
- "output" @ref interface_position2d : robot to be controlled;
  this device must be capable of position control (usually you would
  use the @ref driver_vfh driver)
- @ref interface_map : the map to plan paths in

@par Configuration requests

- PLAYER_PLANNER_GET_WAYPOINTS_REQ

@par Configuration file options

Note that the various thresholds should be set to GREATER than the
underlying position device; otherwise the planner could wait indefinitely
for the position device to achieve a target, when the position device
thinks it has already achieved it.

- safety_dist (length)
  - Default: 0.25 m
  - Don't plan a path any closer than this distance to any obstacle.
    Set this to be GREATER than the corresponding threshold of
    the underlying position device!
- max_radius (length)
  - Default: 1.0 m
  - For planning purposes, all cells that are at least this far from
    any obstacle are equally good (save CPU cycles).
- dist_penalty (float)
  - Default: 1.0
  - Extra cost to discourage cutting corners
- distance_epsilon (length)
  - Default: 0.5 m
  - Planar distance from the target position that will be considered
    acceptable.
    Set this to be GREATER than the corresponding threshold of
    the underlying position device!
- angle_epsilon (angle)
  - Default: 10 deg
  - Angular difference from target angle that will considered acceptable.
    Set this to be GREATER than the corresponding threshold of the
    underlying position device!
- replan_dist_thresh (length)
  - Default: 2.0 m
  - Change in robot's position (in localization space) that will
    trigger replanning.  Set to -1 for no replanning (i.e, make
    a plan one time and then stick with it until the goal is reached).
    Replanning is pretty cheap computationally and can really help in
    dynamic environments.  Note that no changes are made to the map in
    order to replan; support is forthcoming for explicitly replanning
    around obstacles that were not in the map.  See also replan_min_time.
- replan_min_time (float)
  - Default: 2.0
  - Minimum time in seconds between replanning.  Set to -1 for no
    replanning.  See also replan_dist_thresh;
- cspace_file (filename)
  - Default: "player.cspace"
  - Use this file to cache the configuration space (c-space) data.
    At startup, if this file can be read and if the metadata (e.g., size,
    scale) in it matches the current map, then the c-space data is
    read from the file.  Otherwise, the c-space data is computed.
    In either case, the c-space data will be cached to this file for
    use next time.  C-space computation can be expensive and so caching
    can save a lot of time, especially when the planner is frequently
    stopped and started.  This feature requires md5 hashing functions
    in libcrypto.
- add_rotational_waypoints (integer)
  - Default: 1
  - If non-zero, add an in-place rotational waypoint before the next
    waypoint if the difference between the robot's current heading and the
    heading to the next waypoint is greater than 45 degrees.  Generally
    helps the low-level position controller, but sacrifices speed.

@par Example

This example shows how to use the wavefront driver to plan and execute paths
on a laser-equipped Pioneer.

@verbatim
driver
(
  name "p2os"
  provides ["odometry:::position2d:0"]
  port "/dev/ttyS0"
)
driver
(
  name "sicklms200"
  provides ["laser:0"]
  port "/dev/ttyS1"
)
driver
(
  name "mapfile"
  provides ["map:0"]
  filename "mymap.pgm"
  resolution 0.1
)
driver
(
  name "amcl"
  provides ["position2d:2"]
  requires ["odometry:::position2d:1" "laser:0" "laser:::map:0"]
)
driver
(
  name "vfh"
  provides ["position2d:1"]
  requires ["position2d:0" "laser:0"]
  safety_dist 0.1
  distance_epsilon 0.3
  angle_epsilon 5
)
driver
(
  name "wavefront"
  provides ["planner:0"]
  requires ["output:::position2d:1" "input:::position2d:2" "map:0"]
  safety_dist 0.15
  distance_epsilon 0.5
  angle_epsilon 10
)
@endverbatim

@author Brian Gerkey, Andrew Howard
*/
/** @} */

// TODO:
//
//  - allow for computing a path, without actually executing it.
//
//  - compute and return path length

#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>
#include <math.h>

#include <libplayercore/playercore.h>
#include "plan.h"

// TODO: monitor localize timestamps, and slow or stop robot accordingly

// time to sleep between loops (us)
#define CYCLE_TIME_US 100000

class Wavefront : public Driver
{
  private:
    // Main function for device thread.
    virtual void Main();

    // bookkeeping
    player_devaddr_t position_id;
    player_devaddr_t localize_id;
    player_devaddr_t map_id;
    double map_res;
    double robot_radius;
    double safety_dist;
    double max_radius;
    double dist_penalty;
    double dist_eps;
    double ang_eps;
    const char* cspace_fname;

    // the plan object
    plan_t* plan;

    // pointers to the underlying devices
    Device* position;
    Device* localize;
    Device* mapdevice;

    // are we disabled?
    bool enable;
    // current target (m,m,rad)
    double target_x, target_y, target_a;
    // index of current waypoint;
    int curr_waypoint;
    // current waypoint (m,m,rad)
    double waypoint_x, waypoint_y, waypoint_a;
    // current waypoint, in odometric coords (m,m,rad)
    double waypoint_odom_x, waypoint_odom_y, waypoint_odom_a;
    // are we pursuing a new goal?
    bool new_goal;
    // current odom pose
    double position_x, position_y, position_a;
    // current list of waypoints
    double waypoints[PLAYER_PLANNER_MAX_WAYPOINTS][2];
    int waypoint_count;
    // current localize pose
    double localize_x, localize_y, localize_a;
    // have we told the underlying position device to stop?
    bool stopped;
    // have we reached the goal (used to decide whether or not to replan)?
    bool atgoal;
    // replan each time the robot's localization position changes by at
    // least this much (meters)
    double replan_dist_thresh;
    // leave at least this much time (seconds) between replanning cycles
    double replan_min_time;
    // should we request the map at startup? (or wait for it to be pushed
    // to us as data?)
    bool request_map;
    // Do we have a map yet?
    bool have_map;
    // Has the map changed since last time we planned?
    bool new_map;
    // Is there a new map available (which we haven't retrieved yet)?
    bool new_map_available;
    // Do we consider inserting a rotational waypoint between every pair of
    // waypoints, or just before the first one?
    bool always_insert_rotational_waypoints;

    // methods for internal use
    int SetupLocalize();
    int SetupPosition();
    int SetupMap();
    int GetMap(bool threaded);
    int GetMapInfo(bool threaded);
    int ShutdownPosition();
    int ShutdownLocalize();
    int ShutdownMap();
    double angle_diff(double a, double b);

    void ProcessCommand(player_planner_cmd_t* cmd);
    void ProcessLocalizeData(player_position2d_data_t* data);
    void ProcessPositionData(player_position2d_data_t* data);
    void ProcessMapInfo(player_map_info_t* info);
    void PutPositionCommand(double x, double y, double a, unsigned char type);
    void PutPlannerData();
    void StopPosition();
    void LocalizeToPosition(double* px, double* py, double* pa,
                            double lx, double ly, double la);
    void SetWaypoint(double wx, double wy, double wa);

  public:
    // Constructor
    Wavefront( ConfigFile* cf, int section);

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

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


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


// a driver registration function
void Wavefront_Register(DriverTable* table)
{
  table->AddDriver("wavefront",  Wavefront_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
Wavefront::Wavefront( ConfigFile* cf, int section)
  : Driver(cf, section, true,
           PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_PLANNER_CODE)
{
  // Must have a position device to control
  if (cf->ReadDeviceAddr(&this->position_id, section, "requires",
                         PLAYER_POSITION2D_CODE, -1, "output") != 0)
  {
    this->SetError(-1);
    return;
  }
  // Must have a position device from which to read global poses
  if (cf->ReadDeviceAddr(&this->localize_id, section, "requires",
                         PLAYER_POSITION2D_CODE, -1, "input") != 0)
  {
    this->SetError(-1);
    return;
  }
  // Must have a map device
  if (cf->ReadDeviceAddr(&this->map_id, section, "requires",
                         PLAYER_MAP_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);
    return;
  }
  this->safety_dist = cf->ReadLength(section,"safety_dist", 0.25);
  this->max_radius = cf->ReadLength(section,"max_radius",1.0);
  this->dist_penalty = cf->ReadFloat(section,"dist_penalty",1.0);
  this->dist_eps = cf->ReadLength(section,"distance_epsilon", 0.5);
  this->ang_eps = cf->ReadAngle(section,"angle_epsilon",DTOR(10));
  this->replan_dist_thresh = cf->ReadLength(section,"replan_dist_thresh",2.0);
  this->replan_min_time = cf->ReadFloat(section,"replan_min_time",2.0);
  this->request_map = cf->ReadInt(section,"request_map",1);
  this->cspace_fname = cf->ReadFilename(section,"cspace_file","player.cspace");
  this->always_insert_rotational_waypoints =
          cf->ReadInt(section, "add_rotational_waypoints", 1);
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int
Wavefront::Setup()
{
  this->have_map = false;
  this->new_map = false;
  this->new_map_available = false;
  this->stopped = true;
  this->atgoal = true;
  this->enable = true;
  this->target_x = this->target_y = this->target_a = 0.0;
  this->position_x = this->position_y = this->position_a = 0.0;
  this->localize_x = this->localize_y = this->localize_a = 0.0;
  this->waypoint_x = this->waypoint_y = this->waypoint_a = 0.0;
  this->waypoint_odom_x = this->waypoint_odom_y = this->waypoint_odom_a = 0.0;
  this->curr_waypoint = -1;

  this->new_goal = false;

  this->waypoint_count = 0;

  if(SetupPosition() < 0)
    return(-1);

  if(!(this->plan = plan_alloc(this->robot_radius+this->safety_dist,
                               this->robot_radius+this->safety_dist,
                               this->max_radius,
                               this->dist_penalty)))
  {
    PLAYER_ERROR("failed to allocate plan");
    return(-1);
  }

  if(SetupMap() < 0)
    return(-1);
  if(SetupLocalize() < 0)
    return(-1);

  // Start the driver thread.
  this->StartThread();
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int
Wavefront::Shutdown()
{
  // Stop the driver thread.
  this->StopThread();

  if(this->plan)
    plan_free(this->plan);

  ShutdownPosition();
  ShutdownLocalize();
  ShutdownMap();