vfh.cc

机器人仿真软件

    player_msghdr_t* rephdr = msg->GetHeader();
    void* repdata = msg->GetPayload();
    // Copy in our address and forward the response
    rephdr->addr = this->device_addr;
    this->Publish(resp_queue, rephdr, repdata);
    delete msg;
    return(0);
  }
  else
  {
    return -1;
  }
}

////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void VFH_Class::Main()
{
  float dist;
  double angdiff;
  struct timeval startescape, curr;
  bool escaping = false;
  double timediff;
  int escape_turnrate_deg;

  // bookkeeping to implement hysteresis when rotating at the goal
  int rotatedir = 1;

  // bookkeeping to implement smarter escape policy
  int escapedir;

  escapedir = 1;
  while (true)
  {
    // Wait till we get new data
    this->Wait();

    // Test if we are supposed to cancel this thread.
    pthread_testcancel();

    // Process any pending requests.
    this->ProcessMessages();

    if(!this->active_goal)
      continue;

    // Figure how far, in distance and orientation, we are from the goal
    dist = sqrt(pow((goal_x - this->odom_pose[0]),2) +
                pow((goal_y - this->odom_pose[1]),2));
    angdiff = this->angle_diff((double)this->goal_t,this->odom_pose[2]);

    // If we're currently escaping after a stall, check whether we've done
    // so for long enough.
    if(escaping)
    {
      GlobalTime->GetTime(&curr);
      timediff = (curr.tv_sec + curr.tv_usec/1e6) -
              (startescape.tv_sec + startescape.tv_usec/1e6);
      if(timediff > this->escape_time)
      {
        // if we're still stalled, try escaping the other direction
        if(this->odom_stall)
          escapedir *= -1;
        escaping = false;
      }
    }

    // CASE 1: The robot has stalled, so escape if the user specified
    //         a non-zero escape velocity.
    if(escaping ||
       (this->escape_speed && this->escape_time && this->odom_stall))
    {
      if(!escaping)
      {
        GlobalTime->GetTime(&startescape);
        escaping = true;
      }

      this->speed = (int)rint(this->escape_speed * escapedir * 1e3);
      if(this->escape_max_turnspeed)
      {
        // pick a random turnrate in
        // [-escape_max_turnspeed,escape_max_turnspeed]
        escape_turnrate_deg = (int)rint(RTOD(this->escape_max_turnspeed));
        this->turnrate = (int)(2.0 * escape_turnrate_deg *
                               rand()/(RAND_MAX+1.0)) -
                escape_turnrate_deg/2 + 1;
      }
      else
        this->turnrate = 0;
      PutCommand(this->speed, this->turnrate);

      this->turninginplace = false;
    }
    // CASE 2: The robot is at the goal, within user-specified tolerances, so
    //         stop.
    else if((dist < (this->dist_eps * 1e3)) &&
            (fabs(DTOR(angdiff)) < this->ang_eps))
    {
      this->active_goal = false;
      this->speed = this->turnrate = 0;
      PutCommand( this->speed, this->turnrate );
      this->turninginplace = false;
    }
    // CASE 3: The robot is too far from the goal position, so invoke VFH to
    //         get there.
    else if (dist > (this->dist_eps * 1e3))
    {
      float Desired_Angle = (90 + atan2((goal_y - this->odom_pose[1]),
                                        (goal_x - this->odom_pose[0]))
                             * 180 / M_PI - this->odom_pose[2]);

      while (Desired_Angle > 360.0)
        Desired_Angle -= 360.0;
      while (Desired_Angle < 0)
        Desired_Angle += 360.0;

      vfh_Algorithm->Update_VFH( this->laser_ranges,
                                 (int)(this->odom_vel[0]),
                                 Desired_Angle,
                                 dist,
                                 this->dist_eps * 1e3,
                                 this->speed,
                                 this->turnrate );
      
      // HACK: if we're within twice the distance threshold, 
      // and still going fast, slow down.

      if((dist < (this->dist_eps * 1e3 * 2.0)) &&
         (this->speed > (vfh_Algorithm->GetCurrentMaxSpeed() / 2.0)))
      {
//        int foo = this->speed;
        this->speed = 
                (int)rint(vfh_Algorithm->GetCurrentMaxSpeed() / 2.0);
        //printf("slowing down from %d to %d\n",
	//     foo, this->speed);
      }

      PutCommand( this->speed, this->turnrate );
      this->turninginplace = false;
    }
    // CASE 4: The robot is at the goal position, but still needs to turn
    //         in place to reach the desired orientation.
    else
    {
      // At goal, stop
      speed = 0;

      // Turn in place in the appropriate direction, with speed
      // proportional to the angular distance to the goal orientation.
      turnrate = (int)rint(fabs(angdiff/180.0) *
                           vfh_Algorithm->GetMaxTurnrate(speed));

      // If we've just gotten to the goal, pick a direction to turn;
      // otherwise, keep turning the way we started (to prevent
      // oscillation)
      if(!this->turninginplace)
      {
        this->turninginplace = true;
        if(angdiff < 0)
          rotatedir = -1;
        else
          rotatedir = 1;
      }

      turnrate *= rotatedir;

      // Threshold to make sure we don't send arbitrarily small turn speeds
      // (which may not cause the robot to actually move).
      if(turnrate < 0)
        turnrate = MIN(turnrate,-this->vfh_Algorithm->GetMinTurnrate());
      else
        turnrate = MAX(turnrate,this->vfh_Algorithm->GetMinTurnrate());

      this->PutCommand( this->speed, this->turnrate );
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
// Check for new commands from the server
void
VFH_Class::ProcessCommand(player_msghdr_t* hdr, player_position2d_cmd_pos_t &cmd)
{
  int x,y,t;

  x = (int)rint(cmd.pos.px * 1e3);
  y = (int)rint(cmd.pos.py * 1e3);
  t = (int)rint(RTOD(cmd.pos.pa));

  this->cmd_type = 1;
  this->cmd_state = cmd.state;

  if((x != this->goal_x) || (y != this->goal_y) || (t != this->goal_t))
  {
    this->active_goal = true;
    this->turninginplace = false;
    this->goal_x = x;
    this->goal_y = y;
    this->goal_t = t;
  }
}

////////////////////////////////////////////////////////////////////////////////
// Constructor
VFH_Class::VFH_Class( ConfigFile* cf, int section)
  : Driver(cf, section, true,
           PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_POSITION2D_CODE)
{
  double cell_size;
  int window_diameter;
  int sector_angle;
  double safety_dist_0ms;
  double safety_dist_1ms;
  int max_speed;
  int max_speed_narrow_opening;
  int max_speed_wide_opening;
  int max_acceleration;
  int min_turnrate;
  int max_turnrate_0ms;
  int max_turnrate_1ms;
  double min_turn_radius_safety_factor;
  double free_space_cutoff_0ms;
  double obs_cutoff_0ms;
  double free_space_cutoff_1ms;
  double obs_cutoff_1ms;
  double weight_desired_dir;
  double weight_current_dir;


  this->speed = 0;
  this->turnrate = 0;

  cell_size = cf->ReadLength(section, "cell_size", 0.1) * 1e3;
  window_diameter = cf->ReadInt(section, "window_diameter", 61);
  sector_angle = cf->ReadInt(section, "sector_angle", 5);
  safety_dist_0ms = cf->ReadLength(section, "safety_dist_0ms", 0.1) * 1e3;
  safety_dist_1ms = cf->ReadLength(section, "safety_dist_1ms",
                                   safety_dist_0ms/1e3) * 1e3;
  max_speed = (int)rint(1e3 * cf->ReadLength(section, "max_speed", 0.2));
  max_speed_narrow_opening =
          (int)rint(1e3 * cf->ReadLength(section,
                                         "max_speed_narrow_opening",
                                         max_speed/1e3));
  max_speed_wide_opening =
          (int)rint(1e3 * cf->ReadLength(section,
                                         "max_speed_wide_opening",
                                         max_speed/1e3));
  max_acceleration = (int)rint(1e3 * cf->ReadLength(section, "max_acceleration", 0.2));
  min_turnrate = (int)rint(RTOD(cf->ReadAngle(section, "min_turnrate", DTOR(10))));
  max_turnrate_0ms = (int) rint(RTOD(cf->ReadAngle(section, "max_turnrate_0ms", DTOR(40))));
  max_turnrate_1ms =
          (int) rint(RTOD(cf->ReadAngle(section,
                                        "max_turnrate_1ms",
                                        DTOR(max_turnrate_0ms))));
  min_turn_radius_safety_factor =
          cf->ReadFloat(section, "min_turn_radius_safety_factor", 1.0);
  free_space_cutoff_0ms = cf->ReadFloat(section,
                                        "free_space_cutoff_0ms",
                                        2000000.0);
  obs_cutoff_0ms = cf->ReadFloat(section,
                                 "obs_cutoff_0ms",
                                 free_space_cutoff_0ms);
  free_space_cutoff_1ms = cf->ReadFloat(section,
                                        "free_space_cutoff_1ms",
                                        free_space_cutoff_0ms);
  obs_cutoff_1ms = cf->ReadFloat(section,
                                 "obs_cutoff_1ms",
                                 free_space_cutoff_1ms);
  weight_desired_dir = cf->ReadFloat(section, "weight_desired_dir", 5.0);
  weight_current_dir = cf->ReadFloat(section, "weight_current_dir", 3.0);

  this->dist_eps = cf->ReadLength(section, "distance_epsilon", 0.5);
  this->ang_eps = cf->ReadAngle(section, "angle_epsilon", DTOR(10.0));

  this->escape_speed = cf->ReadLength(section, "escape_speed", 0.0);
  this->escape_time = cf->ReadFloat(section, "escape_time", 0.0);
  this->escape_max_turnspeed = cf->ReadAngle(section, "escape_max_turnrate", 0.0);

  // Instantiate the classes that handles histograms
  // and chooses directions
  this->vfh_Algorithm = new VFH_Algorithm(cell_size,
                                          window_diameter,
                                          sector_angle,
                                          safety_dist_0ms,
                                          safety_dist_1ms,
                                          max_speed,
                                          max_speed_narrow_opening,
                                          max_speed_wide_opening,
                                          max_acceleration,
                                          min_turnrate,
                                          max_turnrate_0ms,
                                          max_turnrate_1ms,
                                          min_turn_radius_safety_factor,
                                          free_space_cutoff_0ms,
                                          obs_cutoff_0ms,
                                          free_space_cutoff_1ms,
                                          obs_cutoff_1ms,
                                          weight_desired_dir,
                                          weight_current_dir);
  this->odom = NULL;
  if (cf->ReadDeviceAddr(&this->odom_addr, section, "requires",
                         PLAYER_POSITION2D_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);
    return;
  }

  this->laser = NULL;
  memset(&this->laser_addr,0,sizeof(player_devaddr_t));
  cf->ReadDeviceAddr(&this->laser_addr, section, "requires",
                     PLAYER_LASER_CODE, -1, NULL);
  this->sonar = NULL;
  memset(&this->sonar_addr,0,sizeof(player_devaddr_t));
  cf->ReadDeviceAddr(&this->sonar_addr, section, "requires",
                     PLAYER_SONAR_CODE, -1, NULL);

  if((!this->laser_addr.interf && !this->sonar_addr.interf) ||
     (this->laser_addr.interf && this->sonar_addr.interf))
  {
    PLAYER_ERROR("vfh needs exactly one sonar or one laser");
    this->SetError(-1);
    return;
  }

  // Laser settings
  //TODO this->laser_max_samples = cf->ReadInt(section, "laser_max_samples", 10);

  return;
}


///////////////////////////////////////////////////////////////////////////
// VFH Code


VFH_Class::~VFH_Class() {
  delete this->vfh_Algorithm;

  return;
}

// computes the signed minimum difference between the two angles.  inputs
// and return values are in degrees.
double
VFH_Class::angle_diff(double a, double b)
{
  double ra, rb;
  double d1, d2;

  ra = NORMALIZE(DTOR(a));
  rb = NORMALIZE(DTOR(b));

  d1 = ra-rb;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;

  if(fabs(d1) < fabs(d2))
    return(RTOD(d1));
  else
    return(RTOD(d2));
}