wavefront.cc

机器人仿真软件

      // when making small adjustments to achieve a desired heading (i.e., the
      // robot gets there and VFH stops, but here we don't realize we're done
      // because the localization heading hasn't changed sufficiently).
      if(this->new_goal ||
         (rotate_waypoint &&
          (fabs(this->angle_diff(this->waypoint_odom_a,this->position_a))
           < M_PI/4.0)) ||
         (!rotate_waypoint && (dist < this->dist_eps)))
      {
        if(this->curr_waypoint == this->waypoint_count)
        {
          // no more waypoints, so wait for target achievement

          //puts("waiting for goal achievement");
          usleep(CYCLE_TIME_US);
          continue;
        }
        // get next waypoint
        this->waypoint_x = this->waypoints[this->curr_waypoint][0];
        this->waypoint_y = this->waypoints[this->curr_waypoint][1];
        this->curr_waypoint++;

        this->waypoint_a = this->target_a;
        if(this->always_insert_rotational_waypoints ||
           (this->curr_waypoint == 2))
        {
          dist = sqrt((this->waypoint_x - this->localize_x) *
                      (this->waypoint_x - this->localize_x) +
                      (this->waypoint_y - this->localize_y) *
                      (this->waypoint_y - this->localize_y));
          angle = atan2(this->waypoint_y - this->localize_y,
                        this->waypoint_x - this->localize_x);
          if((dist > this->dist_eps) &&
             fabs(this->angle_diff(angle,this->localize_a)) > M_PI/4.0)
          {
            this->waypoint_x = this->localize_x;
            this->waypoint_y = this->localize_y;
            this->waypoint_a = angle;
            this->curr_waypoint--;
            rotate_waypoint=true;
          }
          else
            rotate_waypoint=false;
        }
        else
          rotate_waypoint=false;

        this->new_goal = false;
      }

      SetWaypoint(this->waypoint_x, this->waypoint_y, this->waypoint_a);
    }


    usleep(CYCLE_TIME_US);
  }
}


////////////////////////////////////////////////////////////////////////////////
// Set up the underlying position device.
int
Wavefront::SetupPosition()
{
  player_position2d_geom_t* geom;
  player_position2d_power_config_t motorconfig;

  // Subscribe to the position device.
  if(!(this->position = deviceTable->GetDevice(this->position_id)))
  {
    PLAYER_ERROR("unable to locate suitable position device");
    return(-1);
  }
  if(this->position->Subscribe(this->InQueue) != 0)
  {
    PLAYER_ERROR("unable to subscribe to position device");
    return(-1);
  }

  Message* msg;
  // Enable the motors
  motorconfig.state = 1;
  if(!(msg = this->position->Request(this->InQueue,
                                     PLAYER_MSGTYPE_REQ,
                                     PLAYER_POSITION2D_REQ_MOTOR_POWER,
                                     (void*)&motorconfig,
                                     sizeof(motorconfig), NULL, false)))
  {
    PLAYER_WARN("failed to enable motors");
  }
  else
    delete msg;

  // Get the robot's geometry
  if(!(msg = this->position->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);
  }

  geom = (player_position2d_geom_t*)msg->GetPayload();

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

  delete msg;

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
// Set up the underlying localize device.
int
Wavefront::SetupLocalize()
{
  // Subscribe to the localize device.
  if(!(this->localize = deviceTable->GetDevice(this->localize_id)))
  {
    PLAYER_ERROR("unable to locate suitable localize device");
    return(-1);
  }
  if(this->localize->Subscribe(this->InQueue) != 0)
  {
    PLAYER_ERROR("unable to subscribe to localize device");
    return(-1);
  }

  return(0);
}

// Retrieve the map data in tiles, assuming that the map info is already
// stored in this->plan.
int
Wavefront::GetMap(bool threaded)
{
  // allocate space for map cells
  this->plan->cells = (plan_cell_t*)realloc(this->plan->cells,
                                            (this->plan->size_x *
                                             this->plan->size_y *
                                             sizeof(plan_cell_t)));
  assert(this->plan->cells);

  // Reset the grid
  plan_reset(this->plan);

  // now, get the map data
  player_map_data_t* data_req;
  size_t reqlen;
  int i,j;
  int oi,oj;
  int sx,sy;
  int si,sj;

  reqlen = sizeof(player_map_data_t) - PLAYER_MAP_MAX_TILE_SIZE;
  data_req = (player_map_data_t*)calloc(1, reqlen);
  assert(data_req);

  // Tile size
  sy = sx = (int)sqrt(PLAYER_MAP_MAX_TILE_SIZE);
  assert(sx * sy < (int)PLAYER_MAP_MAX_TILE_SIZE);
  oi=oj=0;
  while((oi < this->plan->size_x) && (oj < this->plan->size_y))
  {
    si = MIN(sx, this->plan->size_x - oi);
    sj = MIN(sy, this->plan->size_y - oj);

    data_req->col = oi;
    data_req->row = oj;
    data_req->width = si;
    data_req->height = sj;

    Message* msg;
    if(!(msg = this->mapdevice->Request(this->InQueue,
                                        PLAYER_MSGTYPE_REQ,
                                        PLAYER_MAP_REQ_GET_DATA,
                                        (void*)data_req,reqlen,NULL,
                                        threaded)))
    {
      PLAYER_ERROR("failed to get map data");
      free(data_req);
      free(this->plan->cells);
      return(-1);
    }

    player_map_data_t* mapdata = (player_map_data_t*)msg->GetPayload();
    plan_cell_t* cell;

    // copy the map data
    for(j=0;j<sj;j++)
    {
      for(i=0;i<si;i++)
      {
        cell = this->plan->cells + PLAN_INDEX(this->plan,oi+i,oj+j);
        cell->occ_dist = this->plan->max_radius;
        if((cell->occ_state = mapdata->data[j*si + i]) >= 0)
          cell->occ_dist = 0;
      }
    }

    delete msg;

    oi += si;
    if(oi >= this->plan->size_x)
    {
      oi = 0;
      oj += sj;
    }
  }
  free(data_req);
  return(0);
}

int
Wavefront::GetMapInfo(bool threaded)
{
  Message* msg;
  if(!(msg = this->mapdevice->Request(this->InQueue,
                                      PLAYER_MSGTYPE_REQ,
                                      PLAYER_MAP_REQ_GET_INFO,
                                      NULL, 0, NULL, threaded)))
  {
    PLAYER_WARN("failed to get map info");
    this->plan->scale = 0.1;
    this->plan->size_x = 0;
    this->plan->size_y = 0;
    this->plan->origin_x = 0.0;
    this->plan->origin_y = 0.0;
    return(-1);
  }

  player_map_info_t* info = (player_map_info_t*)msg->GetPayload();

  // copy in the map info
  this->plan->scale = info->scale;
  this->plan->size_x = info->width;
  this->plan->size_y = info->height;
  this->plan->origin_x = info->origin.px;
  this->plan->origin_y = info->origin.py;
  // Set the bounds to search the whole grid.
  plan_set_bounds(this->plan,
                  0, 0, this->plan->size_x - 1, this->plan->size_y - 1);

  delete msg;
  return(0);
}

// setup the underlying map device (i.e., get the map)
int
Wavefront::SetupMap()
{
  // Subscribe to the map device
  if(!(this->mapdevice = deviceTable->GetDevice(this->map_id)))
  {
    PLAYER_ERROR("unable to locate suitable map device");
    return(-1);
  }
  if(mapdevice->Subscribe(this->InQueue) != 0)
  {
    PLAYER_ERROR("unable to subscribe to map device");
    return(-1);
  }

  // should we get the map now?  if not, we'll wait for it to be pushed to
  // us as data later.
  if(!this->request_map)
    return(0);

  printf("Wavefront: Loading map from map:%d...\n", this->map_id.index);
  fflush(NULL);

  // Fill in the map structure

  // first, get the map info
  if(this->GetMapInfo(false) < 0)
    return(-1);
  // Now get the map data, possibly in separate tiles.
  if(this->GetMap(false) < 0)
    return(-1);

  plan_update_cspace(this->plan,this->cspace_fname);

  this->have_map = true;
  this->new_map = true;

  puts("Done.");

  return(0);
}

int
Wavefront::ShutdownPosition()
{
  return(this->position->Unsubscribe(this->InQueue));
}

int
Wavefront::ShutdownLocalize()
{
  return(this->localize->Unsubscribe(this->InQueue));
}

int
Wavefront::ShutdownMap()
{
  return(this->mapdevice->Unsubscribe(this->InQueue));
}

////////////////////////////////////////////////////////////////////////////////
// Process an incoming message
int
Wavefront::ProcessMessage(MessageQueue* resp_queue,
                          player_msghdr * hdr,
                          void * data)
{
  // Is it new odometry data?
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                           PLAYER_POSITION2D_DATA_STATE,
                           this->position_id))
  {
    this->ProcessPositionData((player_position2d_data_t*)data);

    // In case localize_id and position_id are the same
    if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                             PLAYER_POSITION2D_DATA_STATE,
                             this->localize_id))
      this->ProcessLocalizeData((player_position2d_data_t*)data);
    return(0);
  }
  // Is it new localization data?
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                                PLAYER_POSITION2D_DATA_STATE,
                                this->localize_id))
  {
    this->ProcessLocalizeData((player_position2d_data_t*)data);
    return(0);
  }
  // Is it a new goal for the planner?
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
                                PLAYER_PLANNER_CMD_GOAL,
                                this->device_addr))
  {
    this->ProcessCommand((player_planner_cmd_t*)data);
    return(0);
  }
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                PLAYER_PLANNER_REQ_GET_WAYPOINTS,
                                this->device_addr))
  {
    player_planner_waypoints_req_t reply;

    if(this->waypoint_count > PLAYER_PLANNER_MAX_WAYPOINTS)
    {
      PLAYER_WARN("too many waypoints; truncating list");
      reply.waypoints_count = 0;
    }
    else
      reply.waypoints_count = this->waypoint_count;
    for(int i=0;i<(int)reply.waypoints_count;i++)
    {
      reply.waypoints[i].px = this->waypoints[i][0];
      reply.waypoints[i].py = this->waypoints[i][1];
      reply.waypoints[i].pa = 0.0;
    }

    this->Publish(this->device_addr, resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_PLANNER_REQ_GET_WAYPOINTS,
                  (void*)&reply, sizeof(reply), NULL);
    return(0);
  }
  // Is it a request to enable or disable the planner?
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
                                PLAYER_PLANNER_REQ_ENABLE,
                                this->device_addr))
  {
    if(hdr->size != sizeof(player_planner_enable_req_t))
    {
      PLAYER_ERROR("incorrect size for planner enable request");
      return(-1);
    }
    player_planner_enable_req_t* enable_req =
            (player_planner_enable_req_t*)data;

    if(enable_req->state)
    {
      this->enable = true;
      PLAYER_MSG0(2,"Robot enabled");
    }
    else
    {
      this->enable = false;
      PLAYER_MSG0(2,"Robot disabled");
    }
    this->Publish(this->device_addr,
                  resp_queue,
                  PLAYER_MSGTYPE_RESP_ACK,
                  PLAYER_PLANNER_REQ_ENABLE);
    return(0);
  }
  // Is it new map metadata?
  else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
                                PLAYER_MAP_DATA_INFO,
                                this->map_id))
  {
    if(hdr->size != sizeof(player_map_info_t))
    {
      PLAYER_ERROR("incorrect size for map info");
      return(-1);
    }
    //this->ProcessMapInfo((player_map_info_t*)data);
    this->new_map_available = true;
    return(0);
  }
  else
    return(-1);
}

// computes the signed minimum difference between the two angles.
double
Wavefront::angle_diff(double a, double b)
{
  double d1, d2;
  a = NORMALIZE(a);
  b = NORMALIZE(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);
}