vfh.cc

机器人仿真软件

  }
  if(this->odom->Subscribe(this->InQueue) != 0)
  {
    PLAYER_ERROR("unable to subscribe to position device");
    return -1;
  }

  // Get the odometry geometry
  Message* msg;
  if(!(msg = this->odom->Request(this->InQueue,
                                 PLAYER_MSGTYPE_REQ,
                                 PLAYER_POSITION2D_REQ_GET_GEOM,
                                 NULL, 0, NULL,false)) ||
     (msg->GetHeader()->size != sizeof(player_position2d_geom_t)))
  {
    PLAYER_ERROR("failed to get geometry of underlying position device");
    if(msg)
      delete msg;
    return(-1);
  }
  player_position2d_geom_t* geom = (player_position2d_geom_t*)msg->GetPayload();

  // take the bigger of the two dimensions and halve to get a radius
  float robot_radius = MAX(geom->size.sl,geom->size.sw);
  robot_radius /= 2.0;

  delete msg;

  vfh_Algorithm->SetRobotRadius( robot_radius );

  this->odom_pose[0] = this->odom_pose[1] = this->odom_pose[2] = 0.0;
  this->odom_vel[0] = this->odom_vel[1] = this->odom_vel[2] = 0.0;
  this->cmd_state = 1;

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Shutdown the underlying odom device.
int VFH_Class::ShutdownOdom()
{

  // Stop the robot before unsubscribing
  this->speed = 0;
  this->turnrate = 0;
  this->PutCommand( speed, turnrate );

  this->odom->Unsubscribe(this->InQueue);
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the laser
int VFH_Class::SetupLaser()
{
  if(!(this->laser = deviceTable->GetDevice(this->laser_addr)))
  {
    PLAYER_ERROR("unable to locate suitable laser device");
    return -1;
  }
  if (this->laser->Subscribe(this->InQueue) != 0)
  {
    PLAYER_ERROR("unable to subscribe to laser device");
    return -1;
  }

  this->laser_count = 0;
  for(int i=0;i<PLAYER_LASER_MAX_SAMPLES;i++)
  {
    this->laser_ranges[i][0] = 0;
    this->laser_ranges[i][1] = 0;
  }
  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the sonar
int VFH_Class::SetupSonar()
{
  if(!(this->sonar = deviceTable->GetDevice(this->sonar_addr)))
  {
    PLAYER_ERROR("unable to locate suitable sonar device");
    return -1;
  }
  if (this->sonar->Subscribe(this->InQueue) != 0)
  {
    PLAYER_ERROR("unable to subscribe to sonar device");
    return -1;
  }

  player_sonar_geom_t* cfg;
  Message* msg;

  // Get the sonar poses
  if(!(msg = this->sonar->Request(this->InQueue,
                                  PLAYER_MSGTYPE_REQ,
                                  PLAYER_SONAR_REQ_GET_GEOM,
                                  NULL, 0, NULL,false)))
  {
    PLAYER_ERROR("failed to get sonar geometry");
    return(-1);
  }

  // Store the sonar poses
  cfg = (player_sonar_geom_t*)msg->GetPayload();
  this->num_sonars = cfg->poses_count;
  for(int i=0;i<this->num_sonars;i++)
  {
    this->sonar_poses[i] = cfg->poses[i];
  }

  delete msg;

  this->laser_count = 0;
  for(int i=0;i<PLAYER_LASER_MAX_SAMPLES;i++)
  {
    this->laser_ranges[i][0] = 0;
    this->laser_ranges[i][1] = 0;
  }
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shut down the laser
int VFH_Class::ShutdownLaser()
{
  this->laser->Unsubscribe(this->InQueue);
  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Shut down the sonar
int VFH_Class::ShutdownSonar()
{
  this->sonar->Unsubscribe(this->InQueue);
  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Process new odometry data
void
VFH_Class::ProcessOdom(player_msghdr_t* hdr, player_position2d_data_t &data)
{

  // Cache the new odometric pose, velocity, and stall info
  // NOTE: this->odom_pose is in (mm,mm,deg), as doubles
  this->odom_pose[0] = data.pos.px * 1e3;
  this->odom_pose[1] = data.pos.py * 1e3;
  this->odom_pose[2] = RTOD(data.pos.pa);
  this->odom_vel[0] = data.vel.px * 1e3;
  this->odom_vel[1] = data.vel.py * 1e3;
  this->odom_vel[2] = RTOD(data.vel.pa);
  this->odom_stall = data.stall;

  // Also change this info out for use by others
  player_msghdr_t newhdr = *hdr;
  newhdr.addr = this->device_addr;
  this->Publish(NULL, &newhdr, (void*)&data);
}

////////////////////////////////////////////////////////////////////////////////
// Process new laser data
void
VFH_Class::ProcessLaser(player_laser_data_t &data)
{
  int i;
  double b, db, r;

  b = RTOD(data.min_angle);
  db = RTOD(data.resolution);

  this->laser_count = 181;
  assert(this->laser_count <
         (int)sizeof(this->laser_ranges) / (int)sizeof(this->laser_ranges[0]));

  for(i = 0; i < PLAYER_LASER_MAX_SAMPLES; i++)
    this->laser_ranges[i][0] = -1;

  // vfh seems to be very oriented around 180 degree scans so interpolate to get 180 degrees
//  b += 90.0;
  for(i = 0; i < 181; i++)
  {
  	unsigned int index = (int)rint(i/db);
  	assert(index >= 0 && index < data.ranges_count);
    this->laser_ranges[i*2][0] = data.ranges[index] * 1e3;
//    this->laser_ranges[i*2][1] = index;
//    b += db;
  }

  r = 1000000.0;
  for (i = 0; i < PLAYER_LASER_MAX_SAMPLES; i++)
  {
    if (this->laser_ranges[i][0] != -1) {
      r = this->laser_ranges[i][0];
    } else {
      this->laser_ranges[i][0] = r;
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
// Process new sonar data, in a very crude way.
void
VFH_Class::ProcessSonar(player_sonar_data_t &data)
{
  int i;
  double b, r;
  double cone_width = 30.0;
  int count = 361;
  float sonarDistToCenter = 0.0;

  this->laser_count = count;
  assert(this->laser_count <
         (int)sizeof(this->laser_ranges) / (int)sizeof(this->laser_ranges[0]));

  for(i = 0; i < PLAYER_LASER_MAX_SAMPLES; i++)
    this->laser_ranges[i][0] = -1;

  //b += 90.0;
  for(i = 0; i < (int)data.ranges_count; i++)
  {
    for(b = RTOD(this->sonar_poses[i].pa) + 90.0 - cone_width/2.0;
        b < RTOD(this->sonar_poses[i].pa) + 90.0 + cone_width/2.0;
        b+=0.5)
    {
      if((b < 0) || (rint(b*2) >= count))
        continue;
      // Sonars give distance readings from the perimeter of the robot while lasers give distance
      // from the laser; hence, typically the distance from a single point, like the center.
      // Since this version of the VFH+ algorithm was written for lasers and we pass the algorithm
      // laser ranges, we must make the sonar ranges appear like laser ranges. To do this, we take
      // into account the offset of a sonar's geometry from the center. Simply add the distance from
      // the center of the robot to a sonar to the sonar's range reading.
      sonarDistToCenter = sqrt(pow(this->sonar_poses[i].px,2) + pow(this->sonar_poses[i].py,2));
      this->laser_ranges[(int)rint(b * 2)][0] = (sonarDistToCenter + data.ranges[i]) * 1e3;
      this->laser_ranges[(int)rint(b * 2)][1] = b;
    }
  }

  r = 1000000.0;
  for (i = 0; i < PLAYER_LASER_MAX_SAMPLES; i++)
  {
    if (this->laser_ranges[i][0] != -1) {
      r = this->laser_ranges[i][0];
    } else {
      this->laser_ranges[i][0] = r;
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
// Send commands to the underlying position device
void
VFH_Class::PutCommand( int cmd_speed, int cmd_turnrate )
{
  player_position2d_cmd_vel_t cmd;

//printf("Command: speed: %d turnrate: %d\n", cmd_speed, cmd_turnrate);

  this->con_vel[0] = (double)cmd_speed;
  this->con_vel[1] = 0;
  this->con_vel[2] = (double)cmd_turnrate;

  memset(&cmd, 0, sizeof(cmd));

  // Stop the robot (locks the motors) if the motor state is set to
  // disabled.  The P2OS driver does not respect the motor state.
  if (this->cmd_state == 0)
  {
    cmd.vel.px = 0;
    cmd.vel.py = 0;
    cmd.vel.pa = 0;
  }
  // Position mode
  else
  {
    if(fabs(this->con_vel[2]) >
       (double)vfh_Algorithm->GetMaxTurnrate((int)this->con_vel[0]))
    {
      PLAYER_WARN1("fast turn %d", this->con_vel[2]);
      this->con_vel[2] = 0;
    }

    cmd.vel.px =  this->con_vel[0] / 1e3;
    cmd.vel.py =  this->con_vel[1] / 1e3;
    cmd.vel.pa =  DTOR(this->con_vel[2]);
  }

  this->odom->PutMsg(this->InQueue,
                     PLAYER_MSGTYPE_CMD,
                     PLAYER_POSITION2D_CMD_VEL,
                     (void*)&cmd,sizeof(cmd),NULL);
}


////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int VFH_Class::ProcessMessage(MessageQueue* resp_queue,
                              player_msghdr * hdr,
                              void * data)
{
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                           PLAYER_POSITION2D_DATA_STATE, this->odom_addr))
  {
    assert(hdr->size == sizeof(player_position2d_data_t));
    ProcessOdom(hdr, *reinterpret_cast<player_position2d_data_t *> (data));
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                                PLAYER_LASER_DATA_SCAN, this->laser_addr))
  {
    // It's not always that big...
    //assert(hdr->size == sizeof(player_laser_data_t));
    ProcessLaser(*reinterpret_cast<player_laser_data_t *> (data));
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                                PLAYER_SONAR_DATA_RANGES, this->sonar_addr))
  {
    // It's not always that big...
    //assert(hdr->size == sizeof(player_laser_data_t));
    ProcessSonar(*reinterpret_cast<player_sonar_data_t *> (data));
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                                PLAYER_POSITION2D_CMD_POS,
                                this->device_addr))
  {
    assert(hdr->size == sizeof(player_position2d_cmd_pos_t));
    ProcessCommand(hdr, *reinterpret_cast<player_position2d_cmd_pos_t *> (data));
    return 0;
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                                PLAYER_POSITION2D_CMD_VEL,
                                this->device_addr))
  {
    assert(hdr->size == sizeof(player_position2d_cmd_vel_t));
    // make a copy of the header and change the address
    player_msghdr_t newhdr = *hdr;
    newhdr.addr = this->odom_addr;
    this->odom->PutMsg(this->InQueue, &newhdr, (void*)data);
    this->cmd_type = 0;
    this->active_goal = false;

    return 0;
  }  
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1, this->device_addr))
  {
    // Pass the request on to the underlying position device and wait for
    // the reply.
    Message* msg;

    if(!(msg = this->odom->Request(this->InQueue,
                                   hdr->type,
                                   hdr->subtype,
                                   (void*)data,
                                   hdr->size,
                                   &hdr->timestamp)))
    {
      PLAYER_WARN1("failed to forward config request with subtype: %d\n",
                   hdr->subtype);
      return(-1);
    }