void gsMapper::drawMap(v3s16 position)
{
// width and height in nodes (these don't really need to be exact)
s16 nwidth = floor(d_width / d_scale);
s16 nheight = floor(d_height / d_scale);
bool hasChanged = false;
// check if position is outside the center
if (d_valid)
{
if ( (position.X - d_border) < d_origin.X ||
(position.Z - d_border) < d_origin.Z ||
(position.X + d_border) > (d_origin.X + nwidth) ||
(position.Z + d_border) > (d_origin.Z + nheight) ) d_valid = false;
}
// recalculate the origin
if (!d_valid || d_tracking)
{
d_origin.X = floor(position.X - (nwidth / 2));
d_origin.Y = d_surface;
d_origin.Z = floor(position.Z - (nheight / 2));
d_valid = true;
hasChanged = true;
}
// rescan next division of the map
Map &map = d_client->getEnv().getMap();
v3s16 p;
s16 x = 0;
s16 y = 0;
s16 z = 0;
while (z < (nheight / 8) && (z + d_scanZ) < nheight)
{
p.Z = d_origin.Z + z + d_scanZ;
x = 0;
while (x < (nwidth / 8) && (x + d_scanX) < nwidth)
{
p.X = d_origin.X + x + d_scanX;
bool b = true;
// "above" mode: surface = mid-point for scanning
if (d_above)
{
p.Y = d_surface + (d_scan / 2);
for (y = 0; y < d_scan; y++)
{
if (b)
{
MapNode n = map.getNodeNoEx(p);
p.Y--;
if (n.param0 != CONTENT_IGNORE && n.param0 != CONTENT_AIR)
{
b = false;
p.Y = d_surface;
// check to see if this node is different from the map
std::map<v3s16, u16>::iterator i2 = d_map.find(p);
if ( i2 == d_map.end() ||
(i2 != d_map.end() && n.param0 != d_map[p]) )
{
hasChanged = true;
d_map[p] = n.param0; // change it
}
}
}
}
// not "above" = use the radar for mapping
} else {
p.Y = position.Y + 1;
MapNode n = map.getNodeNoEx(p);
bool w = (n.param0 != CONTENT_IGNORE && n.param0 != CONTENT_AIR);
int count = 0;
u16 id = CONTENT_AIR;
while (b)
{
if (w) // wall = scan up for air
{
p.Y++;
n = map.getNodeNoEx(p);
if (n.param0 == CONTENT_AIR)
b = false;
else
id = n.param0;
} else { // not wall = scan down for non-air
p.Y--;
n = map.getNodeNoEx(p);
if (n.param0 != CONTENT_IGNORE && n.param0 != CONTENT_AIR)
{
id = n.param0;
b = false;
}
}
if (b && count++ >= (d_scan / 8))
{
b = false;
id = CONTENT_AIR;
}
}
p.Y = d_surface; // the data is always flat
std::map<v3s16, u16>::iterator i5 = d_radar.find(p);
if ( i5 == d_radar.end() ||
(i5 != d_radar.end() && id != d_radar[p]) )
{
hasChanged = true;
d_radar[p] = id; // change it
}
}
x++;
}
z++;
}
// move the scan block
d_scanX += (nwidth / 8);
if (d_scanX >= nwidth)
{
d_scanX = 0;
d_scanZ += (nheight / 8);
if (d_scanZ >= nheight) d_scanZ = 0;
}
video::IVideoDriver *driver = d_device->getVideoDriver();
if (hasChanged || !d_hastex)
{
// set up the image
core::dimension2d<u32> dim(nwidth, nheight);
video::IImage *image = driver->createImage(video::ECF_A8R8G8B8, dim);
assert(image);
bool psum = false;
for (z = 0; z < nheight; z++)
{
for (x = 0; x < nwidth; x++)
{
p.X = d_origin.X + x;
p.Y = d_surface;
p.Z = d_origin.Z + z;
u16 i = CONTENT_IGNORE;
if (d_above)
{
std::map<v3s16, u16>::iterator i3 = d_map.find(p);
if (i3 != d_map.end()) i = d_map[p];
} else {
std::map<v3s16, u16>::iterator i6 = d_radar.find(p);
if (i6 != d_radar.end()) i = d_radar[p];
}
video::SColor c = getColorFromId(i);
c.setAlpha(d_alpha);
image->setPixel(x, nheight - z - 1, c);
if (i != CONTENT_IGNORE) psum = true;
}
}
// image -> texture
if (psum && d_cooldown2 == 0)
{
if (d_hastex)
{
driver->removeTexture(d_texture);
d_hastex = false;
}
std::string f = "gsmapper__" + itos(d_device->getTimer()->getRealTime());
d_texture = driver->addTexture(f.c_str(), image);
assert(d_texture);
d_hastex = true;
d_cooldown2 = 5; // don't generate too many textures all at once
} else {
d_cooldown2--;
if (d_cooldown2 < 0) d_cooldown2 = 0;
}
image->drop();
}
// draw map texture
if (d_hastex) {
driver->draw2DImage( d_texture,
core::rect<s32>(d_posx, d_posy, d_posx+d_width, d_posy+d_height),
core::rect<s32>(0, 0, nwidth, nheight),
0, 0, true );
}
// draw local player marker
if (d_tsrc->isKnownSourceImage("player_marker0.png"))
{
v3s16 p = floatToInt(d_player->getPosition(), BS);
if ( p.X >= d_origin.X && p.X <= (d_origin.X + nwidth) &&
p.Z >= d_origin.Z && p.Z <= (d_origin.Z + nheight) )
{
f32 y = floor(360 - wrapDegrees_0_360(d_player->getYaw()));
if (y < 0) y = 0;
int r = floor(y / 90.0);
int a = floor(fmod(y, 90.0) / 10.0);
std::string f = "player_marker" + itos(a) + ".png";
video::ITexture *pmarker = d_tsrc->getTexture(f);
assert(pmarker);
v2u32 size = pmarker->getOriginalSize();
if (r > 0)
{
core::dimension2d<u32> dim(size.X, size.Y);
video::IImage *image1 = driver->createImage(pmarker,
core::position2d<s32>(0, 0), dim);
assert(image1);
// rotate image
video::IImage *image2 = driver->createImage(video::ECF_A8R8G8B8, dim);
assert(image2);
for (u32 y = 0; y < size.Y; y++)
for (u32 x = 0; x < size.X; x++)
{
video::SColor c;
if (r == 1)
{
c = image1->getPixel(y, (size.X - x - 1));
} else if (r == 2) {
c = image1->getPixel((size.X - x - 1), (size.Y - y - 1));
} else {
c = image1->getPixel((size.Y - y - 1), x);
}
image2->setPixel(x, y, c);
}
image1->drop();
if (d_hasptex)
{
driver->removeTexture(d_pmarker);
d_hasptex = false;
}
d_pmarker = driver->addTexture("playermarker__temp__", image2);
assert(d_pmarker);
pmarker = d_pmarker;
d_hasptex = true;
image2->drop();
}
s32 sx = d_posx + floor((p.X - d_origin.X) * d_scale) - (size.X / 2);
s32 sy = d_posy + floor((nheight - (p.Z - d_origin.Z)) * d_scale) - (size.Y / 2);
driver->draw2DImage( pmarker,
core::rect<s32>(sx, sy, sx+size.X, sy+size.Y),
core::rect<s32>(0, 0, size.X, size.Y),
0, 0, true );
}
}
}
void IDManager::setGLColor(int id) {
col4 col = getColorFromId(id);
glColor4ubv(col.elem);
}
int main(int argc, char **argv) {
ros::init(argc, argv, APP_NAME);
ros::NodeHandle nh;
cv::namedWindow(APP_NAME);
cv::startWindowThread();
image_transport::ImageTransport it(nh);
image_transport::Subscriber sub = it.subscribe("/video", 1, imageCallback);
ros::ServiceServer addColor_service = nh.advertiseService("addColor", addColor);
ros::ServiceServer addPercept_service = nh.advertiseService("addPercept", addPercept);
ros::Publisher percepts_pub = nh.advertise<obj_rec::Percepts>("/percepts", 100);
// Color table management
colorTable.setColorClassSpecifics( 0, cv::Scalar(0,0,0), false);
for( int i = 1; i < 7; i++) {
colorTable.setColorClassSpecifics( i, getColorFromId(i), true);
}
colorTable.setColorClassSpecifics( 7, cv::Scalar(255,255,255), false);
// The main recognition loop
ros::Rate loop_rate(100);
while (ros::ok()) {
if( 0 == frame.empty()) {
boost::mutex::scoped_lock lock(data_locker);
cv::Mat image, seg;
frame.copyTo(image);
frame.copyTo(seg);
// cv::rectangle(seg,cv::Point(0,0), cv::Point(seg.cols, seg.rows), cv::Scalar(0,0,0), -1);
IplImage _lastImage = IplImage(image);
IplImage * lastImage = &_lastImage;
IplImage _segImage = IplImage(seg);
IplImage * segImage = &_segImage;
colorTable.segment(lastImage, segImage, SEG_X, SEG_Y); // segment using the current CT
// Recognition
std::list < Blob > blobs;
blobs = Blob::extractBlobs( lastImage, colorTable, MERGING_DISTANCE_X, MERGING_DISTANCE_Y);
Blob::drawBlobs( colorTable, blobs, segImage, MIN_AREA);
objs = Object::extractObjects( blobs, MIN_AREA, MERGING_DISTANCE_X *2, MERGING_DISTANCE_Y *2);
recognized.clear();
recognized = Object::recognizeObjects( objs, objects_memory);
obj_rec::Percepts percepts_msg;
RecognizedObjects::iterator feat_it;
for( feat_it = recognized.begin(); feat_it != recognized.end(); feat_it++) {
CvScalar color = getColorFromId(feat_it->first);
Object::drawObjects( feat_it->second, segImage, color);
for( Objects::iterator obj_it = feat_it->second.begin();
obj_it != feat_it->second.end();
obj_it++) {
// compose msg
obj_rec::Percept percept_msg;
percept_msg.id = feat_it->first;
percept_msg.u = obj_it->getAvg().x;
percept_msg.v = obj_it->getAvg().y;
percept_msg.width = obj_it->getBottomRight().x - obj_it->getTopLeft().x;
percept_msg.height = obj_it->getBottomRight().y - obj_it->getTopLeft().y;
percept_msg.area = obj_it->getArea();
percepts_msg.percepts.push_back(percept_msg);
}
}
percepts_msg.header = header;
percepts_pub.publish(percepts_msg);
cv::imshow(APP_NAME, seg);
}
ros::spinOnce();
loop_rate.sleep();
}
cv::destroyWindow(APP_NAME);
}
