void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
  unsigned int size_x = new_map->info.width, size_y = new_map->info.height;

  ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);

  // resize costmap if size, resolution or origin do not match
  Costmap2D* master = layered_costmap_->getCostmap();
  if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
      master->getSizeInCellsY() != size_y ||
      master->getResolution() != new_map->info.resolution ||
      master->getOriginX() != new_map->info.origin.position.x ||
      master->getOriginY() != new_map->info.origin.position.y ||
      !layered_costmap_->isSizeLocked()))
  {
    // Update the size of the layered costmap (and all layers, including this one)
    ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
    layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
                                new_map->info.origin.position.y, true);
  }
  else if (size_x_ != size_x || size_y_ != size_y ||
           resolution_ != new_map->info.resolution ||
           origin_x_ != new_map->info.origin.position.x ||
           origin_y_ != new_map->info.origin.position.y)
  {
    // only update the size of the costmap stored locally in this layer
    ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
    resizeMap(size_x, size_y, new_map->info.resolution,
              new_map->info.origin.position.x, new_map->info.origin.position.y);
  }

  unsigned int index = 0;

  // initialize the costmap with static data
  for (unsigned int i = 0; i < size_y; ++i)
  {
    for (unsigned int j = 0; j < size_x; ++j)
    {
      unsigned char value = new_map->data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }
  map_frame_ = new_map->header.frame_id;

  // we have a new map, update full size of map
  x_ = y_ = 0;
  width_ = size_x_;
  height_ = size_y_;
  map_received_ = true;
  has_updated_data_ = true;

  // shutdown the map subscrber if firt_map_only_ flag is on
  if (first_map_only_)
  {
    ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
    map_sub_.shutdown();
  }
}
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
{
  unsigned int di = 0;
  for (unsigned int y = 0; y < update->height ; y++)
  {
    unsigned int index_base = (update->y + y) * size_x_;
    for (unsigned int x = 0; x < update->width ; x++)
    {
      unsigned int index = index_base + x + update->x;
      costmap_[index] = interpretValue(update->data[di++]);
    }
  }
  x_ = update->x;
  y_ = update->y;
  width_ = update->width;
  height_ = update->height;
  has_updated_data_ = true;
}
예제 #3
0
int CredsCommonsInnerBackend::getMethodFromConfigurationFile(const CredentialsMap &methodCodeMap,
                                                             const std::string &methodName,
                                                             int &method) {
    std::ifstream f(credsConfigurationFile());
    if (!f.is_open())
        return CYNARA_API_CONFIGURATION_ERROR;

    std::locale loc = f.getloc();
    bool occurred = false;
    std::string key, value;
    int tmpMethod;
    while (getKeyAndValue(f, loc, key, value))
        if (key == methodName && !interpretValue(methodCodeMap, tmpMethod, value, occurred))
            return CYNARA_API_CONFIGURATION_ERROR;
    if (occurred) {
        method = tmpMethod;
        return CYNARA_API_SUCCESS;
    }
    return CYNARA_API_CONFIGURATION_ERROR;
}
예제 #4
0
파일: hostcodeIR.c 프로젝트: svenvh/arduino
int main() {
  FILE *serial = fopen(SERIALFILE, "r+");

  if (!serial) {
    fprintf(stderr, "Could not open '%s'\n", SERIALFILE);
    exit(1);
  }

  unsigned int value = 0;

  while (1) {
    int matched = fscanf(serial, "%x", &value);
    if (matched == 1) {
      interpretValue(value);
      value = 0;
    }
    usleep(DELAY_USEC);
  }

  fclose(serial);
  return 0;
}