vfh.cc

机器人仿真软件

#include <assert.h>
#include <math.h>
#include <fstream>
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <unistd.h>
#include <errno.h>
#include <time.h>

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

extern PlayerTime *GlobalTime;

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_vfh vfh
 * @brief Vector Field Histogram local navigation algorithm

@note This driver may take several seconds to start up, especially on slower machines.  You may want to set the 'alwayson' option for vfh to '1' in your configuration file in order to front-load this delay.  Otherwise, your client may experience a timeout in trying to subscribe to this device.

The vfh driver implements the Vector Field Histogram Plus local
navigation method by Ulrich and Borenstein.  VFH+ provides real-time
obstacle avoidance and path following capabilities for mobile robots.
Layered on top of a laser-equipped robot, vfh works great as a local
navigation system (for global navigation, you can layer the @ref
driver_wavefront driver on top of vfh).

The primary parameters to tweak to get reliable performance are
safety_dist and free_space_cutoff.  In general, safety_dist determines how
close the robot will come to an obstacle while turning (around a corner
for instance) and free_space_cutoff determines how close a robot will
get to an obstacle in the direction of motion before turning to avoid.
From experience, it is recommeded that max_turnrate should be at least
15% of max_speed.

To get initiated to VFH, I recommend starting with the default
values for all parameters and experimentally adjusting safety_dist
and free_space_cutoff to get a feeling for how the parameters affect
performance.  Once comfortable, increase max_speed and max_turnrate.
Unless you are familiar with the VFH algorithm, I don't recommend
deviating from the default values for cell_size, window_diameter,
or sector_angle.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_position2d : accepts target poses, to which vfh will
  attempt to drive the robot.  Also passes through the data from the
  underlying @ref interface_position2d device.  All data and commands
  are in the odometric frame of the underlying device.

@par Requires

- @ref interface_position2d : the underlying robot that will be
  controlled by vfh.

- @ref interface_laser : the laser that will be used to avoid
  obstacles

- @todo : add support for getting the robot's true global pose via the
  @ref interface_simulation interface

@par Configuration requests

- all position2d requests (as long as the underlying position2d device
  supports them)

@par Supported commands

- PLAYER_POSITION2D_CMD_POS : Position control.  This is the normal way to use vfh.  Velocity commands will be sent to the underlying @ref interface_position2d device to drive it toward the given pose.
- PLAYER_POSITION2D_CMD_VEL : Velocity control.  Position control is disabled and the velocities are passed directly through to the underlyin @ref interface_position2d device.

@par Configuration file options

- cell_size (length)
  - Default: 0.1 m
  - Local occupancy map grid size
- window_diameter (integer)
  - Default: 61
  - Dimensions of occupancy map (map consists of window_diameter X
    window_diameter cells).
- sector_angle (integer)
  - Default: 5
  - Histogram angular resolution, in degrees.
- safety_dist_0ms (length)
  - Default: 0.1 m
  - The minimum distance the robot is allowed to get to obstacles when stopped.
- safety_dist_1ms (length)
  - Default: safety_dist_0ms
  - The minimum distance the robot is allowed to get to obstacles when
    travelling at 1 m/s.
- max_speed (length / sec)
  - Default: 0.2 m/sec
  - The maximum allowable speed of the robot.
- max_speed_narrow_opening (length / sec)
  - Default: max_speed
  - The maximum allowable speed of the robot through a narrow opening
- max_speed_wide_opening (length / sec)
  - Default: max_speed
  - The maximum allowable speed of the robot through a wide opening
- max_acceleration (length / sec / sec)
  - Default: 0.2 m/sec/sec
  - The maximum allowable acceleration of the robot.
- min_turnrate (angle / sec)
  - Default: 10 deg/sec
  - The minimum allowable turnrate of the robot.
- max_turnrate_0ms (angle / sec)
  - Default: 40 deg/sec
  - The maximum allowable turnrate of the robot when stopped.
- max_turnrate_1ms (angle / sec)
  - Default: max_turnrate_0ms
  - The maximum allowable turnrate of the robot when travelling 1 m/s.
- min_turn_radius_safety_factor (float)
  - Default: 1.0
  - ?
- free_space_cutoff_0ms (float)
  - Default: 2000000.0
  - Unitless value.  The higher the value, the closer the robot will
    get to obstacles before avoiding (while stopped).
- free_space_cutoff_1ms (float)
  - Default: free_space_cutoff_0ms
  - Unitless value.  The higher the value, the closer the robot will
    get to obstacles before avoiding (while travelling at 1 m/s).
- obs_cutoff_0ms (float)
  - Default: free_space_cutoff_0ms
  - ???
- obs_cutoff_1ms (float)
  - Default: free_space_cutoff_1ms
  - ???
- weight_desired_dir (float)
  - Default: 5.0
  - Bias for the robot to turn to move toward goal position.
- weight_current_dir (float)
  - Default: 3.0
  - Bias for the robot to continue moving in current direction of travel.
- distance_epsilon (length)
  - Default: 0.5 m
  - Planar distance from the target position that will be considered
    acceptable.
- angle_epsilon (angle)
  - Default: 10 deg
  - Angular difference from target angle that will considered acceptable.
- Stall escape options.  If the underlying position2d device reports a
  stall, this driver can attempt a blind escape procedure.  It does so by
  driving forward or backward while turning for a fixed amount of time.  If
  the escape fails (i.e., the stall is still in effect), then it will try again.
  - escape_speed (length / sec)
    - Default: 0.0
    - If non-zero, the translational velocity that will be used while trying
      to escape.
  - escape_time (float)
    - Default: 0.0
    - If non-zero, the time (in seconds) for which an escape attempt will be 
      made.
  - escape_max_turnspeed (angle / sec)
    - Default: 0.0
    - If non-zero, the maximum angular velocity that will be used when 
      trying to escape.

@par Example
@verbatim
driver
(
  name "p2os"
  provides ["odometry::position:1"]
  port "/dev/ttyS0"
)
driver
(
  name "sicklms200"
  provides ["laser:0"]
  port "/dev/ttyS1"
)
driver
(
  name "vfh"
  requires ["position:1" "laser:0"]
  provides ["position:0"]
  safety_dist 0.10
  distance_epsilon 0.3
  angle_epsilon 5
)
@endverbatim

@author Chris Jones, Brian Gerkey, Alex Brooks

*/

/** @} */

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

    // Destructor
    virtual ~VFH_Class();

    // 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);
    // Main function for device thread.
    virtual void Main();

  private:
    bool active_goal;
    bool turninginplace;

    // Set up the odometry device.
    int SetupOdom();
    int ShutdownOdom();
    void ProcessOdom(player_msghdr_t* hdr, player_position2d_data_t &data);

    // Class to handle the internal VFH algorithm
    // (like maintaining histograms etc)
    VFH_Algorithm *vfh_Algorithm;

    // Process requests.  Returns 1 if the configuration has changed.
    //int HandleRequests();
    // Handle motor power requests
    void HandlePower(void *client, void *req, int reqlen);
    // Handle geometry requests.
    void HandleGetGeom(void *client, void *req, int reqlen);

    // Set up the laser device.
    int SetupLaser();
    int ShutdownLaser();
    int SetupSonar();
    int ShutdownSonar();
    void ProcessLaser(player_laser_data_t &);
    void ProcessSonar(player_sonar_data_t &);

    // Send commands to underlying position device
    void PutCommand( int speed, int turnrate );

    // Check for new commands from server
    void ProcessCommand(player_msghdr_t* hdr, player_position2d_cmd_pos_t &);

    // Computes the signed minimum difference between the two angles.  Inputs
    // and return values are in degrees.
    double angle_diff(double a, double b);

    // Odometry device info
    Device *odom;
    player_devaddr_t odom_addr;
    double dist_eps;
    double ang_eps;

    // how fast and how long to back up to escape from a stall
    double escape_speed;
    double escape_max_turnspeed;
    double escape_time;

    // Pose and velocity of robot in odometric cs (mm,mm,deg)
    double odom_pose[3];
    double odom_vel[3];

    // Stall flag and counter
    int odom_stall;

    // Laser device info
    Device *laser;
    player_devaddr_t laser_addr;

    // Sonar device info
    Device *sonar;
    player_devaddr_t sonar_addr;
    int num_sonars;
    player_pose_t sonar_poses[PLAYER_SONAR_MAX_SAMPLES];

    // Laser range and bearing values
    int laser_count;
    double laser_ranges[PLAYER_LASER_MAX_SAMPLES][2];

    // Control velocity
    double con_vel[3];

    int speed, turnrate;
    double reset_odom_x, reset_odom_y, reset_odom_t;
    int32_t goal_x, goal_y, goal_t;
    int cmd_state, cmd_type;
};

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

// a driver registration function
void VFH_Register(DriverTable* table)
{
  table->AddDriver("vfh",  VFH_Init);
  return;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int VFH_Class::Setup()
{
  this->active_goal = false;
  this->turninginplace = false;
  this->goal_x = this->goal_y = this->goal_t = 0;


  // Initialise the underlying position device.
  if (this->SetupOdom() != 0)
    return -1;

  // Initialise the laser.
  if (this->laser_addr.interf && this->SetupLaser() != 0)
    return -1;
  if (this->sonar_addr.interf && this->SetupSonar() != 0)
    return -1;

  // FIXME
  // Allocate and intialize
  vfh_Algorithm->Init();

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

  return 0;
}

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

  // Stop the laser
  if(this->laser)
    this->ShutdownLaser();

  // Stop the sonar
  if(this->sonar)
    this->ShutdownSonar();

  // Stop the odom device.
  this->ShutdownOdom();

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the underlying odom device.
int VFH_Class::SetupOdom()
{
  if(!(this->odom = deviceTable->GetDevice(this->odom_addr)))
  {
    PLAYER_ERROR("unable to locate suitable position device");
    return -1;