/*
KITECH_TopologicalGlobalMapComp의 onInitialize()에서
bmp 파일의 비트맵 읽어 세선화 알고리즘을 수행하여 처리하기 때문에
수행속도가 오래 걸림.
bmp 비트맵의 크기와 형태에 따라 세선화 알고리즘의 수행속도가 틀려짐.
<환경 조건>
1. Tetra 로봇 플렛폼
2. Intel Core2Duo 2.1GHz
3. 950 x 850 크기의 map
<실험>
1. onInitialize() 수행 속도 측정
26047ms
26078ms
26093ms
26125ms
26031ms
26031ms
26094ms
26078ms
26125ms
26047ms
*/
ReturnType KITECH_TopologicalGlobalMapComp::onInitialize()
{
PrintMessage ("KITECH_TopologicalGlobalMapComp::onInitialize()\n");
#define GET_PIXEL(image, x, y) (*(image + mapWidth * y + x))
#define SET_PIXEL(image, x, y, value) (*(image + mapWidth * y + x) = value)
parameter.SetProperty(getPropertyMap());
if (!LoadProperty()) {
error = -1;
return OPROS_FIND_PROPERTY_ERROR;
}
gridBasedGlobalMap = new GridBasedGlobalMap (mapFileName.c_str(), mapResolution);
if (gridBasedGlobalMap == NULL) {
error = -1;
return OPROS_INTERNAL_FAULT;
}
gridBasedGlobalMap->ExpandObstacle (1, 1, 255);
gridBasedGlobalMap->ExpandObstacle (1, 255, 127);
gridBasedGlobalMap->ExpandObstacle (1, 127, 63);
gridBasedGlobalMap->ExpandObstacle (INTEGER(mobileSafeRadius/mapResolution), 1, 10);
int mapWidth = gridBasedGlobalMap->_width;
int mapHeight = gridBasedGlobalMap->_height;
BYTE *inBuff = new BYTE [mapWidth*mapHeight];
BYTE *outBuff = new BYTE [mapWidth*mapHeight];
int x, y;
for (y = 0; y < mapHeight; y++) {
for (x = 0; x < mapWidth; x++) {
BYTE pixel = gridBasedGlobalMap->GetPixelCELL (x, y);
SET_PIXEL (inBuff, x, y, ((0 < pixel) ? 0 : 4));
}
}
CThinning thinnig;
if (thinnig.MakeThinning (outBuff, inBuff, mapWidth, mapHeight) < 0) {
delete [] inBuff;
delete [] outBuff;
error = -1;
return OPROS_INTERNAL_FAULT;
}
for (y = 0; y < mapHeight; y++) {
for (x = 0; x < mapWidth; x++) {
BYTE pixel = GET_PIXEL (outBuff, x, y);
BYTE pixel2 = gridBasedGlobalMap->GetPixelCELL (x, y);
gridBasedGlobalMap->SetPixelCELL (x, y, pixel | pixel2);
}
}
delete [] inBuff;
delete [] outBuff;
error = 0;
return OPROS_SUCCESS;
}
Mesh* ModelLoader::LoadMesh(BinaryReader &br)
{
Mesh *mesh = new Mesh();
mesh ->id = br.Read<int>();
mesh->name = br.Read<std::string>();
if (m_fileVersion > 0)
{
for (int i = 0; i < 16; i++)
mesh->m_worldInverseMatrix.a[i] = br.Read<float>();
mesh->m_worldMatrix = mesh->m_worldInverseMatrix.GetInversed();
}
int meshPartsCount = br.Read<int>();
for (int i = 0; i < meshPartsCount; i++)
{
MeshPart *meshPart = LoadMeshPart(br);
meshPart->mesh = mesh;
mesh ->AddMeshPart(meshPart);
}
int propsCount = br.Read<int>();
for (int i = 0; i < propsCount; i++)
{
Property *prop = LoadProperty(br);
mesh->m_properties[prop->GetName()] = prop;
}
return mesh;
}
// Call back Declaration
ReturnType KITECH_MonteCarloLocalizationComp::onInitialize()
{
PrintMessage ("KITECH_MonteCarloLocalizationComp::onInitialize()\n");
parameter.SetProperty(getPropertyMap());
if (!LoadProperty ()) {
return OPROS_FIND_PROPERTY_ERROR;
}
error = 0;
return OPROS_SUCCESS;
}
// Call back Declaration
ReturnType KITECH_OccupancyGridGlobalMapComp::onInitialize()
{
PrintMessage ("KITECH_OccupancyGridGlobalMapComp::onInitialize()\n");
parameter.SetProperty(getPropertyMap());
if (!LoadProperty()) {
error = -1;
return OPROS_FIND_PROPERTY_ERROR;
}
occupancyGridMap = new OccupancyGridMap (mapWidth, mapHeight, mapResolution,
laserSensorCount, laserSensorStartAngle, laserSensorEndAngle, laserSensorMinimumRange, laserSensorMaximumRange);
if (occupancyGridMap == NULL) {
error = -1;
return OPROS_INTERNAL_FAULT;
}
error = 0;
return OPROS_SUCCESS;
}
bool CModelToolCharDataTable::LoadPropertyList(const char *lpszPath, const char * lpszFileName)
{
CNtlXMLDoc doc;
std::string strPath( lpszPath );
std::string strFileName;
if(doc.Create() == false)
{
_ASSERT(0);
return false;
}
strFileName = strPath;
strFileName += "/";
strFileName += lpszFileName;
if(doc.Load( const_cast<char *>(strFileName.c_str()) ) == false)
{
_ASSERT(0);
return false;
}
IXMLDOMNodeList *pList = doc.SelectNodeList(XML_PROP_LIST_ELEMENT);
IXMLDOMNode* pNode = NULL;
char szBuffer[1024];
long lNum;
pList->get_length(&lNum);
for(long i = 0; i < lNum ; ++i)
{
pList->get_item(i,&pNode);
if(!doc.GetTextWithAttributeName(pNode, XML_PROP_LIST_ELEMENT_NAME_ATTRIBUTE, szBuffer, sizeof(szBuffer)))
{
_ASSERT(0);
return false;
}
strFileName = strPath + "/" + szBuffer;
if( LoadProperty( strFileName.c_str() ) == false )
{
_ASSERT(0);
return false;
}
if(pNode)
{
pNode->Release();
pNode = NULL;
}
}
if(pList)
{
pList->Release();
pList = NULL;
}
return true;
}
AttrList::AttrList (PropertyName n) : (n) {
value = nil;
type = nil;
}
boolean AttrList::FindAttr (register PropertyDef& p) {
register AttrList* a;
for (a = (AttrList*)next; a != this; a = (AttrList*)a->next) {
if (a->name == p.name) {
p.type = a->type;
p.value = a->value;
return true;
}
}
return false;
}
/*
* class DirList
*/
DirList::DirList () {
info = nil;
}
DirList::DirList (PropertyName n) : (n) {
info = new PropDir;
}
boolean DirList::FindSubDir (PropertyName n, PropDir*& subdir) {
register DirList* d;
for (d = (DirList*)next; d != this; d = (DirList*)d->next) {
if (d->name == n) {
subdir = d->info;
return true;
}
}
return false;
}
DirList::~DirList () {
delete info;
}
/*
* class PropDir
*/
PropDir::PropDir () {
parent = nil;
attrs = new AttrList;
dirs = new DirList;
vattrs = new AttrList;
vdirs = new DirList;
}
PropDir::~PropDir () {
dirs->DeleteAll();
delete dirs;
vdirs->DeleteAll();
delete vdirs;
attrs->DeleteAll();
delete attrs;
vattrs->DeleteAll();
delete vattrs;
}
PropDir* PropDir::MakeDirs (const char*& name) {
register const char* p;
PropDir* dir;
DirList* cur, * sub;
PropertyName n;
int c;
dir = this;
c = '.';
for (p = name; *p != '\0'; p++) {
if (*p == '.' || *p == '*') {
if (p > name) {
n = nameTable->Id(name, p - name);
cur = (c == '*') ? dir->vdirs : dir->dirs;
if (!cur->FindSubDir(n, dir)) {
sub = new DirList(n);
sub->info->parent = dir;
cur->Append(sub);
dir = sub->info;
}
}
c = *p;
name = p + 1;
}
}
return dir;
}
/*
* class PropPath
*/
PropPath::PropPath () {
top = &element[0];
next = nil;
prev = nil;
}
/*
* class PropertySheet
*/
PropertySheet::PropertySheet () {
cur = new PropDir;
head = new PropPath;
tail = head;
head->element[0].dir = cur;
head->element[0].sibling = false;
if (propvalues == nil) {
propvalues = new StringPool;
}
}
PropertySheet::~PropertySheet () {
register PropPath* p, * pnext;
delete cur;
for (p = head; p != nil; p = pnext) {
pnext = p->next;
delete p;
}
}
boolean PropertySheet::Get (PropertyDef& prop) {
register PropPath* p;
register PropPathElement* i;
/* look for instance.attr */
p = tail;
i = tail->top;
if (i->dir->attrs->FindAttr(prop)) {
return true;
}
/* look for class.attr (if prev path element is class for instance) */
if (i->sibling) {
/* prev path element is class -- look for class.attr */
--i;
if (i < &p->element[0]) {
i = p->prev->top;
}
if (i->dir->attrs->FindAttr(prop)) {
return true;
}
}
/* look for path*attr */
for (p = tail; p != nil; p = p->prev) {
for (i = p->top; i >= &p->element[0]; i--) {
if (i->dir->vattrs->FindAttr(prop)) {
return true;
}
}
}
prop.value = nil;
return false;
}
/*
* Special lookup under current directory for .attr or *attr.
*/
boolean PropertySheet::GetLocal (PropDir* dir, PropertyDef& prop) {
if (dir->attrs->FindAttr(prop) || dir->vattrs->FindAttr(prop)) {
return true;
}
prop.value = nil;
return false;
}
/*
* Add an empty directory to the property sheet.
*/
PropDir* PropertySheet::MakeDir (const char* path) {
const char* name = path;
PropDir* dir = cur;
DirList* d = (name > path && *(name-1) == '*') ? dir->vdirs : dir->dirs;
PropertyName n = nameTable->Id(name);
if (!d->FindSubDir(n, dir)) {
DirList* sub = new DirList(n);
sub->info->parent = dir;
d->Append(sub);
dir = sub->info;
}
return dir;
}
/*
* Put an attribute in the property sheet, starting the path search
* at a given directory (typically the root). If the name is already
* defined and the "override" parameter is false, then do nothing.
*/
void PropertySheet::DoPut (
PropDir* root, const char* path, const char* value, const char* type,
boolean override
) {
const char* name = path;
boolean newvalue = override;
PropDir* dir = root->MakeDirs(name);
AttrList* alist =
(name > path && *(name-1) == '*') ? dir->vattrs : dir->attrs;
PropertyName n = nameTable->Id(name);
register AttrList* a = (AttrList*)alist->Find(n);
if (a == nil) {
a = new AttrList(n);
alist->Append(a);
newvalue = true;
}
if (newvalue) {
register const char* v;
for (v = value; isspace(*v); v++);
a->value = propvalues->Append(v, (int)(strlen(v) + 1));
a->type = type;
}
}
/*
* Look up the path for a subdirectory matching the given name.
*/
PropDir* PropertySheet::Find (PropertyName name) {
register PropPath* p;
register PropPathElement* e;
PropDir* dir;
dir = nil;
/* look for path.name */
if (tail->top->dir->dirs->FindSubDir(name, dir)) {
return dir;
}
/* look for path*name */
for (p = tail; p != nil; p = p->prev) {
for (e = p->top; e >= &p->element[0]; e--) {
if (e->dir->vdirs->FindSubDir(name, dir)) {
return dir;
}
}
}
return nil;
}
void PropertySheet::Push (PropDir* dir, boolean b) {
register PropPath* p;
register PropPathElement* e;
p = tail;
e = p->top + 1;
if (e >= &p->element[pathClusterSize]) {
p = new PropPath;
p->prev = tail;
tail->next = p;
tail = p;
e = p->top;
} else {
p->top = e;
}
e->dir = dir;
e->sibling = b;
}
void PropertySheet::Pop () {
register PropPath* p = tail;
if (p == nil) {
/* ignore underflow */
} else if (p->top > &p->element[0]) {
p->top -= 1;
} else {
tail = p->prev;
tail->next = nil;
delete p;
}
}
PropDir* PropertySheet::Root () {
return cur;
}
static int line; /* for error handling */
/*
* Load a single property from a string containing
* the name and value, terminated by either a newline or null.
*/
class PropSheetBuf {
public:
PropSheetBuf(int);
~PropSheetBuf();
char* data() { return _data; }
private:
char* _data;
char _smallbuf[256];
};
PropSheetBuf::PropSheetBuf(int n) {
_data = (n < sizeof(_smallbuf)) ? _smallbuf : new char[n + 1];
}
PropSheetBuf::~PropSheetBuf() {
if (_data != _smallbuf) {
delete _data;
}
}
void PropertySheet::LoadProperty (const char* s) {
register const char* src;
register char* dst;
const char* value;
for (src = s; *src == ' ' || *src == '\t'; src++);
if (*src == '#' || *src == '\n' || *src == '\0') {
return;
}
PropSheetBuf buf(strlen(src));
dst = buf.data();
value = nil;
for (; *src != '\n' && *src != '\0'; src++) {
if (*src == '\\') {
++src;
if (*src == 'n') {
*dst++ = '\n';
} else if (*src == '\n') {
++line;
} else {
*dst++ = *src;
}
} else if (value == nil && *src == ':') {
if (dst == buf.data()) {
fprintf(stderr, "%d: empty path\n", line);
return;
}
*dst++ = '\0';
value = dst;
} else {
*dst++ = *src;
}
}
*dst = '\0';
if (value == nil) {
fprintf(stderr, "%d: missing value for %s\n", line, buf.data());
return;
}
PutLower(buf.data(), value);
}
/*
* Parse property sheet information from a string.
*/
void PropertySheet::LoadList (const char* data) {
register const char* p, * start;
start = data;
for (p = strchr(data, '\n'); p != nil; p = strchr(p+1, '\n')) {
if (p > start && *(p-1) != '\\') {
LoadProperty(start);
}
start = p+1;
}
}
/*
* Read a property sheet from a file (e.g., Xdefaults).
*/
boolean PropertySheet::LoadFile (const char* filename) {
FILE* f;
char buf[4096];
register int i;
if (filename == nil) {
f = stdin;
} else {
f = fopen(filename, "r");
if (f == nil) {
return false;
}
}
line = 0;
i = 0;
while (fgets(&buf[i], (int)(sizeof(buf) - i), f) != nil) {
i = strlen(buf);
if (buf[i-1] == '\n' && buf[i-2] != '\\') {
LoadProperty(buf);
i = 0;
} else {
if (i == sizeof(buf)) {
fprintf(stderr, "%s: %d: line too long\n", filename, line);
return true;
}
}
}
return true;
}
bool KITECH_OccupancyGridGlobalMapComp::SetParameter(Property parameter)
{
this->parameter = parameter;
return LoadProperty();
}
