void WorldDrawer::drawParticles(Particle averageParticle, std::vector<Particle> * particles)
{
for(unsigned int i = 0; i < particles->size(); i++)
{
int r, g, b;
getHeatMapColor(particles->at(i).weight, r, g, b);
drawSample(particles->at(i).position, CV_RGB(r, g, b));
}
drawSample(WorldModel::currentPose, CV_RGB(255, 255, 255));
drawSample(averageParticle.position, CV_RGB(255, 0, 0));
}
void River::generateImages(QVector<QImage> &images, QVector<QString> & stockNames,
QMutex &imageMutex, Statistics & stats)
{
imageMutex.lock();
#pragma omp parallel
{
#pragma omp for
for(int imageIndex = 0; imageIndex < NUM_IMAGES; imageIndex++){
QColor color("black");
images[imageIndex].fill(color.rgb());
}
#pragma omp for
for(int i = 0; i < p.getSize(); i++){
if(!p.hasWater[i]) {
continue;
}
int x = p.pxcor[i];
int y = p.pycor[i];
QColor macroColor = getHeatMapColor(p.macro[i], stats.avgMacro, stats.maxMacro);
QColor phytoColor = getHeatMapColor(p.phyto[i], stats.avgPhyto, stats.maxPhyto);
QColor herbivoreColor = getHeatMapColor(p.herbivore[i], stats.avgHerbivore, stats.maxHerbivore);
QColor waterDecompColor = getHeatMapColor(p.waterdecomp[i], stats.avgWaterDecomp, stats.maxWaterDecomp);
QColor sedDecompColor = getHeatMapColor(p.seddecomp[i], stats.avgSedDecomp, stats.maxSedDecomp);
QColor sedConsumerColor = getHeatMapColor(p.sedconsumer[i], stats.avgSedConsumer, stats.maxSedConsumer);
QColor consumColor = getHeatMapColor(p.consumer[i], stats.avgConsum, stats.maxConsum);
QColor DOCColor = getHeatMapColor(p.DOC[i], stats.avgDOC, stats.maxDOC);
QColor POCColor = getHeatMapColor(p.POC[i], stats.avgPOC, stats.maxPOC);
QColor detritusColor = getHeatMapColor(p.detritus[i], stats.avgDetritus, stats.maxDetritus);
images[STOCK_MACRO].setPixel( x, y, macroColor.rgb());
images[STOCK_PHYTO].setPixel(x, y, phytoColor.rgb());
images[STOCK_HERBIVORE].setPixel(x, y, herbivoreColor.rgb());
images[STOCK_WATERDECOMP].setPixel( x, y, waterDecompColor.rgb());
images[STOCK_SEDDECOMP].setPixel(x, y, sedDecompColor.rgb());
images[STOCK_SEDCONSUMER].setPixel(x, y, sedConsumerColor.rgb());
images[STOCK_CONSUMER].setPixel(x, y, consumColor.rgb());
images[STOCK_DOC].setPixel(x, y, DOCColor.rgb());
images[STOCK_POC].setPixel(x, y, POCColor.rgb());
images[STOCK_DETRITUS].setPixel(x, y, detritusColor.rgb());
int patchCarbon = p.macro[i] + p.phyto[i] + p.herbivore[i] + p.waterdecomp[i] + p.seddecomp[i]
+ p.sedconsumer[i] + p.consumer[i] + p.DOC[i] + p.POC[i] + p.detritus[i];
QColor allCarbonColor = getHeatMapColor(patchCarbon, stats.avgCarbon, stats.maxCarbon);
images[STOCK_ALL_CARBON].setPixel(x, y, allCarbonColor.rgb());
}
//Due to the layout of the hydrofiles, the river will appear upside down if we don't flip it.
#pragma omp for
for(int imageIndex = 0; imageIndex < NUM_IMAGES; imageIndex++){
images[imageIndex] = images[imageIndex].mirrored(false,true);
}
}
imageMutex.unlock();
QImageWriter writer;
writer.setFormat("png");
for(int i = 0; i < NUM_IMAGES; i++){
QString date_time_str = QDateTime::currentDateTime().toString("_MMM_d_H_mm_ss");
QString fileName = "./results/images/" + stockNames[i] + date_time_str + ".png";
writer.setFileName(fileName);
writer.write(images[i]);
}
}
void map_cb(const nav_msgs::OccupancyGridConstPtr& map)
{
ROS_INFO("Received a %d X %d map @ %.3f m/pix",
map->info.width,
map->info.height,
map->info.resolution);
std::string mapdatafile = mapname + ".ppm";
ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
FILE* out = fopen(mapdatafile.c_str(), "w");
if (!out)
{
ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
return;
}
fprintf(out, "P6\n# CREATOR: mapsaver.cpp %.3f m/pix\n%d %d\n255\n",
map->info.resolution, map->info.width, map->info.height);
min = 1000;
max = 0;
for(unsigned int y = 0; y < map->info.height; y++)
{
for(unsigned int x = 0; x < map->info.width; x++)
{
unsigned int i = x + (map->info.height - y - 1) * map->info.width;
if (map->data[i] == -1)
continue;
else
{
min = (map->data[i] < min) ? map->data[i] : min;
max = (map->data[i] > max) ? map->data[i] : max;
}
}
}
std::cout << "min: " << min << std::endl;
std::cout << "max: " << max << std::endl;
for(unsigned int y = 0; y < map->info.height; y++)
{
for(unsigned int x = 0; x < map->info.width; x++)
{
unsigned int i = x + (map->info.height - y - 1) * map->info.width;
double new_data = 255*(map->data[i] - min)/(max - min);
std::cout << (int) map->data[i] << " ";
if (new_data >= 255 || map->data[i] == -1)
{
fputc(255, out);
fputc(255, out);
fputc(255, out);
}
else if (new_data <= 0)
{
fputc(0, out);
fputc(0, out);
fputc(0, out);
}
else
{
float red = 0;
float green = 0;
float blue = 0;
getHeatMapColor(new_data, &red, &green, &blue);
fputc((int) red, out);
fputc((int) green, out);
fputc((int) blue, out);
}
}
std::cout << std::endl;
}
fclose(out);
savedMap = true;
ROS_INFO("Done\n");
}
