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