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;
}
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;
}
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;
}