hint32 hdata_set_tree(hdata_set_t data_set, hdata_set_tree_key_t key,hdata_set_tree_key_t parent_key,hash_code_t key_hash,equal_t key_equal,list_compare_t comparator,hany param,InvokeTickDeclare){
data_set_t * ds = (data_set_t *)data_set;
hany k;
hany pk;
hint32 i,c;
hdata_t items,item;
data_set_tree_item_t * tree_item,*tree_pitem;
data_set_tree_item_compare_param_t compare_param;
if(ds && key && parent_key){
data_set_tree_item_dealloc(ds->tree_root,InvokeTickArg);
ds->tree_root = data_set_tree_item_alloc( NULL, ext_data(ds->data),NULL,NULL,NULL,InvokeTickArg);
ds->tree_level = 0;
if(ds->tree_cache == NULL){
ds->tree_cache = map_alloc( key_hash, key_equal);
}
else{
map_clear(ds->tree_cache);
}
items = ext_data(ds->data);
c = hdata_array_size(ext_data_class(ds->data), items);
for(i=0;i<c;i++){
item = hdata_array(ext_data_class(ds->data),items,i);
k = (*key)(data_set,item,param,InvokeTickArg);
pk = (*parent_key)(data_set,item,param,InvokeTickArg);
tree_pitem = map_get(ds->tree_cache, pk);
tree_item = data_set_tree_item_alloc( k, item, tree_pitem == NULL?ds->tree_root:tree_pitem,comparator,param,InvokeTickArg);
data_set_tree_item_dealloc( map_put(ds->tree_cache, k, tree_item),InvokeTickArg);
if(ds->tree_level < tree_item->level){
ds->tree_level = tree_item->level;
}
}
c = list_count(ds->tree_root->childs);
for(i=0;i<c;i++){
tree_item = list_get(ds->tree_root->childs, i);
pk = (*parent_key)(data_set,tree_item->data,param,InvokeTickArg);
tree_pitem = map_get(ds->tree_cache, pk);
if(tree_pitem){
list_remove_at(ds->tree_root->childs, i--);
c = list_count(ds->tree_root->childs);
if(tree_pitem->childs==NULL){
tree_pitem->childs = list_alloc(20, 20);
}
if(comparator){
compare_param.compare = comparator;
compare_param.param = param;
list_add_and_order(tree_pitem->childs, tree_item, data_set_tree_item_compare, &compare_param);
}
else{
list_add(tree_pitem->childs,tree_item);
}
tree_item->level = tree_pitem->level +1;
if(ds->tree_level < tree_item->level){
ds->tree_level = tree_item->level;
}
}
}
return ds->tree_level;
}
return 0;
}
map_t * requestCSpaceMap(const char *srv_name, const int free_threshold,
const int occupied_threshold) {
ros::NodeHandle nh;
// Taken from PAMCL
map_t* map = map_alloc();
ROS_ASSERT(map);
// get map via RPC
nav_msgs::GetMap::Request req;
nav_msgs::GetMap::Response resp;
ROS_INFO("Requesting the map...");
while(!ros::service::call(srv_name, req, resp)) {
ROS_WARN("Request for map '%s' failed; trying again...",
ros::names::resolve(string(srv_name)).c_str());
ros::Duration d(4.0);
d.sleep();
if (!nh.ok()) {
return NULL;
}
}
ROS_INFO_ONCE("Received a %d X %d map @ %.3f m/pix\n",
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution);
convertMap(resp.map, map, free_threshold, occupied_threshold);
return map;
}
struct himemce_module *
map_add_module (struct himemce_map *map, wchar_t *filename, void *base)
{
struct himemce_module *mod;
int len;
int idx;
if (map->nr_modules == HIMEMCE_MAP_MAX_MODULES)
{
ERR ("too many modules\n");
return NULL;
}
mod = &map->module[map->nr_modules];
len = wcslen (filename);
mod->filename = map_alloc (map, (len + 1) * sizeof (wchar_t));
if (! mod->filename)
return NULL;
wcscpy (mod->filename, filename);
idx = len;
while (idx > 0 && mod->filename[idx - 1] != '\\'
&& mod->filename[idx - 1] != '/')
idx--;
mod->dllname = &mod->filename[idx];
mod->base = base;
len = WideCharToMultiByte (CP_UTF8, 0, mod->dllname, -1, NULL, 0, NULL, NULL);
if (len == 0)
{
ERR ("conversion failure: %i\n", GetLastError ());
return NULL;
}
mod->name = map_alloc (map, len);
if (! mod->name)
return NULL;
if (WideCharToMultiByte (CP_UTF8, 0, mod->dllname, -1, mod->name, len,
NULL, NULL) != len)
{
ERR ("conversion inconsistency: %i\n", GetLastError ());
return NULL;
}
map->nr_modules++;
return mod;
}
bimap_t bimap_alloc(size_t size, size_t key_1_size, size_t key_2_size, size_t data_size, key_cmp_f less_1, key_cmp_f less_2) {
bimap_t bimap = (bimap_t)malloc(sizeof(struct bimap_t_));
bimap->size = size;
bimap->data_size = data_size;
bimap->key_1_size = key_1_size;
bimap->key_2_size = key_2_size;
bimap->storage_size = data_size + key_1_size + key_2_size;
bimap->count = 0;
bimap->key_1_map = map_alloc(size, key_1_size, sizeof(size_t), less_1);
bimap->key_2_map = map_alloc(size, key_2_size, sizeof(size_t), less_2);
bimap->datas = malloc(size * bimap->storage_size);
return bimap;
}
void OccupancyMap::setMap(const nav_msgs::OccupancyGrid &grid) {
if (map_ != NULL) {
map_free(map_);
}
map_ = map_alloc();
ROS_ASSERT(map_);
convertMap(grid, map_, max_free_threshold_, min_occupied_threshold_);
}
hdata_t hdata_object_alloc(InvokeTickDeclare){
data_t * d = (data_t *)mem_malloc( sizeof(data_t));
memset(d,0,sizeof(data_t));
d->version = DATA_STRUCT_VERSION;
d->type = HDATA_TYPE_OBJECT;
d->value.object_value = map_alloc( hash_code_str, equal_str);
return (hdata_t)d;
}
void make_chunk(Chunk *chunk, int p, int q) {
chunk->p = p;
chunk->q = q;
chunk->faces = 0;
Map *map = &chunk->map;
map_alloc(map);
make_world(map, p, q);
update_chunk(chunk);
}
int main(int argc, char **argv)
{
map_grid_t **dungeon;
int x, y;
{
struct timeval tv;
gettimeofday(&tv, NULL);
srandom(tv.tv_sec + tv.tv_usec);
}
dungeon = map_alloc(MAP_SIZE_X, MAP_SIZE_Y);
generate_dungeon(dungeon, MAP_SIZE_X, MAP_SIZE_Y, 2, 0);
for (y = 0; y < MAP_SIZE_Y; y++)
{
for (x = 0; x < MAP_SIZE_X; x++)
{
switch(dungeon[x][y].feature)
{
case TERRAIN_WALL:
{
printf("#");
break;
}
case TERRAIN_WALL_O:
{
printf("%");
break;
}
case TERRAIN_FLOOR:
{
printf(".");
break;
}
case TERRAIN_DOOR:
{
printf("+");
break;
}
}
}
printf("\n");
}
map_free(dungeon);
return(0);
}
void do_lab(t_option *opt)
{
char **map;
map = map_alloc(opt);
map_ext(opt, map);
way_ext(opt, map);
my_putstr("AUTEUR : folie_a\n");
my_put_nbr(opt->w);
my_putchar(opt->dd);
my_put_nbr(opt->h);
my_putchar('\n');
my_showtab(map);
}
/* Allocate 2^@order contiguous pages. Returns a VIRTUAL address. */
unsigned long alloc_pages(int order)
{
int i;
chunk_head_t *alloc_ch, *spare_ch;
chunk_tail_t *spare_ct;
/* Find smallest order which can satisfy the request. */
for ( i = order; i < FREELIST_SIZE; i++ ) {
if ( !FREELIST_EMPTY(free_head[i]) )
break;
}
if ( i == FREELIST_SIZE ) goto no_memory;
/* Unlink a chunk. */
alloc_ch = free_head[i];
free_head[i] = alloc_ch->next;
alloc_ch->next->pprev = alloc_ch->pprev;
/* We may have to break the chunk a number of times. */
while ( i != order )
{
/* Split into two equal parts. */
i--;
spare_ch = (chunk_head_t *)((char *)alloc_ch + (1UL<<(i+PAGE_SHIFT)));
spare_ct = (chunk_tail_t *)((char *)spare_ch + (1UL<<(i+PAGE_SHIFT)))-1;
/* Create new header for spare chunk. */
spare_ch->level = i;
spare_ch->next = free_head[i];
spare_ch->pprev = &free_head[i];
spare_ct->level = i;
/* Link in the spare chunk. */
spare_ch->next->pprev = &spare_ch->next;
free_head[i] = spare_ch;
}
map_alloc(PHYS_PFN(to_phys(alloc_ch)), 1UL<<order);
return((unsigned long)alloc_ch);
no_memory:
printk("Cannot handle page request order %d!\n", order);
return 0;
}
void create_chunk(Chunk *chunk, int p, int q) {
chunk->p = p;
chunk->q = q;
chunk->faces = 0;
chunk->dirty = 1;
chunk->position_buffer = 0;
chunk->normal_buffer = 0;
chunk->uv_buffer = 0;
Map *map = &chunk->map;
map_alloc(map);
create_world(map, p, q);
db_load_map(map, p, q);
gen_chunk_buffers(chunk);
client_chunk(p, q);
}
static void data_xpath_selector_handle(data_xpath_selector_t *selector,InvokeTickDeclare){
if(selector && selector->data && selector->child_tags ==NULL){
hdata_type_code type = hdata_type(&hdata_xml_class,selector->data);
hint32 i,c;
if(selector->child_tags ==NULL){
selector->child_tags = map_alloc( hash_code_str, equal_str);
}
if(type==HDATA_TYPE_OBJECT){
data_xpath_selector_handle_child_tags(selector,selector->data,InvokeTickArg);
}
else if(type == HDATA_TYPE_ARRAY){
c = hdata_array_size(&hdata_xml_class, selector->data);
for(i=0;i<c;i++){
data_xpath_selector_handle_child_tags(selector,hdata_array(&hdata_xml_class,selector->data,i),InvokeTickArg);
}
}
}
}
/* Handle an HTTP request. Works out whether Tomcat will be interested then either
* despatches it to Tomcat or passes it back to Domino.
*/
static unsigned int ParsedRequest(FilterContext *context, FilterParsedRequest *reqData)
{
unsigned int errID;
int rc;
FilterRequest fr;
int result = kFilterNotHandled;
DEBUG(("\nParsedRequest starting\n"));
rc = context->GetRequest(context, &fr, &errID);
if (fr.URL && strlen(fr.URL))
{
char *uri = fr.URL;
char *workerName, *qp, *turi;
if (!initDone)
{
/* One time initialisation which is deferred so that we have the name of
* the server software to plug into worker_env
*/
int ok = JK_FALSE;
jk_map_t *map = NULL;
DEBUG(("Initialising worker map\n"));
if (map_alloc(&map))
{
if (map_read_properties(map, workerMountFile))
if (uri_worker_map_alloc(&uw_map, map, logger))
ok = JK_TRUE;
map_free(&map);
}
DEBUG(("Got the URI worker map\n"));
if (ok)
{
ok = JK_FALSE;
DEBUG(("About to allocate map\n"));
if (map_alloc(&map))
{
DEBUG(("About to read %s\n", workerFile));
if (map_read_properties(map, workerFile))
{
#if defined(JK_VERSION) && JK_VERSION >= MAKEVERSION(1, 2, 0, 1)
char server[256];
worker_env.uri_to_worker = uw_map;
if (context->GetServerVariable(context, "SERVER_SOFTWARE", server, sizeof(server)-1, &errID))
worker_env.server_name = jk_pool_strdup(&cfgPool, server);
else
worker_env.server_name = SERVERDFLT;
DEBUG(("Server name %s\n", worker_env.server_name));
if (wc_open(map, &worker_env, logger))
ok = JK_TRUE;
#else
if (wc_open(map, logger))
ok = JK_TRUE;
#endif
DEBUG(("OK = %d\n", ok));
}
DEBUG(("Read %s, OK = %d\n", workerFile, ok));
map_free(&map);
}
}
if (!ok) return kFilterError;
initDone = JK_TRUE;
}
turi = strdup(uri);
if (qp = strchr(turi, '?'), tqp != NULL) *qp = '\0';
workerName = map_uri_to_worker(uw_map, turi, logger);
free(turi);
DEBUG(("Worker for this URL is %s\n", workerName));
if (NULL != workerName)
{
private_ws_t ws;
jk_ws_service_t s;
jk_pool_atom_t buf[SMALL_POOL_SIZE];
if (BadURI(uri))
return RejectBadURI(context);
/* Go dispatch the call */
jk_init_ws_service(&s);
jk_open_pool(&ws.p, buf, sizeof (buf));
ws.responseStarted = JK_FALSE;
ws.context = context;
ws.reqData = reqData;
ws.reqSize = context->GetRequestContents(context, &ws.reqBuffer, &errID);
s.ws_private = &ws;
s.pool = &ws.p;
if (InitService(&ws, &s))
{
jk_worker_t *worker = wc_get_worker_for_name(workerName, logger);
jk_log(logger, JK_LOG_DEBUG, "HttpExtensionProc %s a worker for name %s\n",
worker ? "got" : "could not get", workerName);
if (worker)
{
jk_endpoint_t *e = NULL;
if (worker->get_endpoint(worker, &e, logger))
{
int recover = JK_FALSE;
if (e->service(e, &s, logger, &recover))
{
result = kFilterHandledRequest;
jk_log(logger, JK_LOG_DEBUG, "HttpExtensionProc service() returned OK\n");
DEBUG(("HttpExtensionProc service() returned OK\n"));
}
else
{
result = kFilterError;
jk_log(logger, JK_LOG_ERROR, "HttpExtensionProc error, service() failed\n");
DEBUG(("HttpExtensionProc error, service() failed\n"));
}
e->done(&e, logger);
}
}
else
{
jk_log(logger, JK_LOG_ERROR,
"HttpExtensionProc error, could not get a worker for name %s\n",
workerName);
}
}
jk_close_pool(&ws.p);
}
}
return result;
}
static void
bootset(int fd)
{
uuid_t uuid;
off_t block;
off_t size;
unsigned int entry;
map_t *gpt, *tpg;
map_t *tbl, *lbt;
map_t *map;
u_int32_t status;
struct gpt_hdr *hdr;
struct gpt_ent *ent;
struct mbr *mbr;
int bfd;
/*
* Paramters for boot partition
*/
uuid_name_lookup(&uuid, "DragonFly Label32", &status);
if (status != uuid_s_ok)
err(1, "unable to find uuid for 'DragonFly Label32'");
entry = 0;
block = 0;
size = 768 * 1024 * 1024 / 512;
gpt = map_find(MAP_TYPE_PRI_GPT_HDR);
if (gpt == NULL)
errx(1, "%s: error: no primary GPT header", device_name);
tpg = map_find(MAP_TYPE_SEC_GPT_HDR);
if (tpg == NULL)
errx(1, "%s: error: no secondary GPT header", device_name);
tbl = map_find(MAP_TYPE_PRI_GPT_TBL);
lbt = map_find(MAP_TYPE_SEC_GPT_TBL);
if (tbl == NULL || lbt == NULL) {
errx(1, "%s: error: no primary or secondary gpt table",
device_name);
}
hdr = gpt->map_data;
if (entry > le32toh(hdr->hdr_entries)) {
errx(1, "%s: error: index %u out of range (%u max)",
device_name, entry, le32toh(hdr->hdr_entries));
}
ent = (void *)((char *)tbl->map_data + entry *
le32toh(hdr->hdr_entsz));
if (!uuid_is_nil(&ent->ent_type, NULL)) {
errx(1, "%s: error: entry at index %d is not free",
device_name, entry);
}
map = map_alloc(block, size);
if (map == NULL)
errx(1, "%s: error: no space available on device", device_name);
block = map->map_start;
size = map->map_size;
le_uuid_enc(&ent->ent_type, &uuid);
ent->ent_lba_start = htole64(map->map_start);
ent->ent_lba_end = htole64(map->map_start + map->map_size - 1LL);
hdr->hdr_crc_table = htole32(crc32(tbl->map_data,
le32toh(hdr->hdr_entries) *
le32toh(hdr->hdr_entsz)));
hdr->hdr_crc_self = 0;
hdr->hdr_crc_self = htole32(crc32(hdr, le32toh(hdr->hdr_size)));
gpt_write(fd, gpt);
gpt_write(fd, tbl);
hdr = tpg->map_data;
ent = (void*)((char*)lbt->map_data + entry * le32toh(hdr->hdr_entsz));
le_uuid_enc(&ent->ent_type, &uuid);
ent->ent_lba_start = htole64(map->map_start);
ent->ent_lba_end = htole64(map->map_start + map->map_size - 1LL);
hdr->hdr_crc_table = htole32(crc32(lbt->map_data,
le32toh(hdr->hdr_entries) *
le32toh(hdr->hdr_entsz)));
hdr->hdr_crc_self = 0;
hdr->hdr_crc_self = htole32(crc32(hdr, le32toh(hdr->hdr_size)));
gpt_write(fd, lbt);
gpt_write(fd, tpg);
/*
* Create a dummy partition
*/
map = map_find(MAP_TYPE_PMBR);
if (map == NULL)
errx(1, "I can't find the PMBR!");
mbr = map->map_data;
if (mbr == NULL)
errx(1, "I can't find the PMBR's data!");
/*
* Copy in real boot code
*/
bfd = open("/boot/boot0", O_RDONLY);
if (bfd < 0 ||
read(bfd, mbr->mbr_code, sizeof(mbr->mbr_code)) !=
sizeof(mbr->mbr_code)) {
errx(1, "Cannot read /boot/boot0");
}
close(bfd);
/*
* Generate partition #1
*/
mbr->mbr_part[1].part_shd = 0xff;
mbr->mbr_part[1].part_ssect = 0xff;
mbr->mbr_part[1].part_scyl = 0xff;
mbr->mbr_part[1].part_ehd = 0xff;
mbr->mbr_part[1].part_esect = 0xff;
mbr->mbr_part[1].part_ecyl = 0xff;
mbr->mbr_part[1].part_start_lo = htole16(block);
mbr->mbr_part[1].part_start_hi = htole16((block) >> 16);
mbr->mbr_part[1].part_size_lo = htole16(size);
mbr->mbr_part[1].part_size_hi = htole16(size >> 16);
mbr->mbr_part[1].part_typ = 165;
mbr->mbr_part[1].part_flag = 0x80;
gpt_write(fd, map);
}
FDBConfig * FDBConfigCreate(hext_data_t data){
InvokeTickBegin
FDBConfigInternal * cfg = (FDBConfigInternal *) malloc(sizeof(FDBConfigInternal));
memset(cfg, 0, sizeof(FDBConfigInternal));
cfg->dbClasss = list_alloc(32, 32);
cfg->indexs = list_alloc(32, 32);
cfg->dbClassMap = map_alloc(hash_code_str, equal_str);
cfg->indexMap = map_alloc(hash_code_str, equal_str);
{
hdata_t items = ext_data_object(data, "class");
hint32 c = hdata_array_size(ext_data_class(data), items);
hint32 i,size,pc,pi;
hdata_t item,propertys,property;
FDBClassConfig * config;
FDBProperty * prop;
hcchar * type;
for(i=0;i<c;i++){
config = malloc(sizeof(FDBClassConfig));
memset(config, 0, sizeof(FDBClassConfig));
item = hdata_array(ext_data_class(data), items, i);
propertys = hdata_object(ext_data_class(data), item, "propertys");
strncpy(config->name, hdata_object_string(ext_data_class(data), item, "name", ""), sizeof(config->name));
size = sizeof(FDBClass) + hdata_array_size(ext_data_class(data), propertys) * sizeof(FDBProperty);
config->dbClass = (FDBClass *) malloc(size);
memset(config->dbClass, 0, size);
config->dbClass->propertyCount = hdata_array_size(ext_data_class(data), propertys) + 1;
pc = config->dbClass->propertyCount;
prop = & config->dbClass->rowid;
strcpy(prop->name, "rowid");
prop->type = FDBPropertyTypeInt32;
prop ++;
pc --;
pi = 0;
while(pc >0){
property = hdata_array(ext_data_class(data), propertys, pi);
strncpy(prop->name, hdata_object_string(ext_data_class(data), property, "name", ""), sizeof(prop->name));
type = hdata_object_string(ext_data_class(data), property, "type", "string");
if(strcmp(type, "int32") ==0){
prop->type = FDBPropertyTypeInt32;
}
else if(strcmp(type, "int64") ==0){
prop->type = FDBPropertyTypeInt64;
}
else if(strcmp(type, "double") ==0){
prop->type = FDBPropertyTypeDouble;
}
else if(strcmp(type, "blob") ==0){
prop->type = FDBPropertyTypeBlob;
}
else {
prop->type = FDBPropertyTypeString;
prop->length = hdata_object_int32(ext_data_class(data), property, "length", 64);
}
pc --;
prop ++;
pi ++;
}
FDBClassInitialize(config->dbClass);
list_add(cfg->dbClasss, config);
map_put(cfg->dbClassMap, config->name, config);
}
}
{
hdata_t items = ext_data_object(data, "index");
hint32 c = hdata_array_size(ext_data_class(data), items);
hint32 i,size,pc,pi;
hdata_t item,propertys,property;
FDBIndexConfig * config;
FDBProperty * prop;
FDBIndexSortProperty * sortProperty;
hcchar * type;
for(i=0;i<c;i++){
config = malloc(sizeof(FDBIndexConfig));
memset(config, 0, sizeof(FDBIndexConfig));
item = hdata_array(ext_data_class(data), items, i);
propertys = hdata_object(ext_data_class(data), item, "propertys");
strncpy(config->name, hdata_object_string(ext_data_class(data), item, "name", ""), sizeof(config->name));
size = sizeof(FDBIndex) + hdata_array_size(ext_data_class(data), propertys) * sizeof(FDBProperty);
config->index = (FDBIndex *) malloc(size);
memset(config->index, 0, size);
config->index->propertyCount = hdata_array_size(ext_data_class(data), propertys) + 1;
pc = config->index->propertyCount;
prop = & config->index->rowid;
strcpy(prop->name, "rowid");
prop->type = FDBPropertyTypeInt32;
prop ++;
pc --;
pi = 0;
while(pc >0){
property = hdata_array(ext_data_class(data), propertys, pi);
strncpy(prop->name, hdata_object_string(ext_data_class(data), property, "name", ""), sizeof(prop->name));
type = hdata_object_string(ext_data_class(data), property, "type", "string");
if(strcmp(type, "int32") ==0){
prop->type = FDBPropertyTypeInt32;
}
else if(strcmp(type, "int64") ==0){
prop->type = FDBPropertyTypeInt64;
}
else if(strcmp(type, "double") ==0){
prop->type = FDBPropertyTypeDouble;
}
else {
prop->type = FDBPropertyTypeString;
prop->length = hdata_object_int32(ext_data_class(data), property, "length", 64);
}
pc --;
prop ++;
pi ++;
}
FDBIndexInitialize(config->index);
propertys = hdata_object(ext_data_class(data), item, "sort");
config->sortPropertysCount = hdata_array_size(ext_data_class(data), propertys);
if(config->sortPropertysCount >0){
config->sortPropertys = malloc(sizeof(FDBIndexSortProperty) * config->sortPropertysCount);
memset(config->sortPropertys , 0, sizeof(FDBIndexSortProperty) * config->sortPropertysCount);
sortProperty = config->sortPropertys;
for(pi = 0;pi <config->sortPropertysCount;pi ++){
property = hdata_array(ext_data_class(data), propertys, pi);
sortProperty[pi].property = FDBIndexGetProperty(config->index,hdata_object_string(ext_data_class(data), property, "name", ""));
type = hdata_object_string(ext_data_class(data), property, "mode", "asc");
if(strcmp(type, "desc") ==0){
sortProperty[pi].mode = FDBIndexCompareOrderDesc;
}
else{
sortProperty[pi].mode = FDBIndexCompareOrderAsc;
}
}
}
list_add(cfg->indexs, config);
map_put(cfg->indexMap, config->name,config);
}
}
cfg->base.classCount = list_count(cfg->dbClasss);
cfg->base.indexCount = list_count(cfg->indexs);
return (FDBConfig *)cfg;
}
int
generic_decode_fail (const struct test_case *tests,
unsigned ntests,
size_t data_size,
int (ASN1CALL *decode)(unsigned char *, size_t, void *, size_t *))
{
unsigned char *buf;
int i;
int failures = 0;
void *data;
struct map_page *data_map, *buf_map;
#ifdef HAVE_SIGACTION
struct sigaction sa, osa;
#endif
for (i = 0; i < ntests; ++i) {
int ret;
size_t sz;
const void *bytes;
current_test = tests[i].name;
current_state = "init";
#ifdef HAVE_SIGACTION
sigemptyset (&sa.sa_mask);
sa.sa_flags = 0;
#ifdef SA_RESETHAND
sa.sa_flags |= SA_RESETHAND;
#endif
sa.sa_handler = segv_handler;
sigaction (SIGSEGV, &sa, &osa);
#endif
data = map_alloc(OVERRUN, NULL, data_size, &data_map);
if (tests[i].byte_len < 0xffffff && tests[i].byte_len >= 0) {
sz = tests[i].byte_len;
bytes = tests[i].bytes;
} else {
sz = 4096;
bytes = NULL;
}
buf = map_alloc(OVERRUN, bytes, sz, &buf_map);
if (tests[i].byte_len == -1)
memset(buf, 0, sz);
current_state = "decode";
ret = (*decode) (buf, tests[i].byte_len, data, &sz);
if (ret == 0) {
printf ("sucessfully decoded %s\n", tests[i].name);
++failures;
continue;
}
current_state = "free";
if (buf)
map_free(buf_map, tests[i].name, "encode");
map_free(data_map, tests[i].name, "data");
#ifdef HAVE_SIGACTION
sigaction (SIGSEGV, &osa, NULL);
#endif
}
current_state = "done";
return failures;
}
int * map_alloc(void);
task_context_t * cars_alloc(void);
light_state * lights_alloc(void);
void light_changer_start(void);
int main(int __attribute__((unused))argc, __attribute__((unused)) char **argv) {
int i;
char buf[50];
char * dirname = get_current_dir_name();
snprintf(buf, 50, "%s/%s",dirname,argv[0]);
free(dirname);
mykey = ftok(buf,0);
semid = semget( mykey, 1, IPC_CREAT | 0660 );
map_unlock();
//mykey = ftok(buf,1);
map = map_alloc();
//mykey = ftok(buf,2);
cars = cars_alloc();
//mykey = ftok(buf,3);
lights = lights_alloc();
printf("map address %p\n",(void*)map);
printf("cars address %p\n",(void*)cars);
printf("lights address %p\n",(void*)lights);
light_changer_start();
for (i = 0; i < MAX_CARS ; i++) {
cars[i].id = 1 + i;
cars[i].alive = 1;
}
seed = time(NULL);
for (i = 0; i < MAX_CARS; i++) {
/**
* Convert an OccupancyGrid map message into the internal representation.
*/
void AMCLocalizer::initMap( const nav_msgs::OccupancyGrid& map_msg ) {
/*
* Convert an OccupancyGrid map message into the internal representation.
*/
std::cout << "1" << std::endl;
freeMapDependentMemory();
// Clear queued laser objects because they hold pointers to the existing map, #5202.
// lasers_.clear();
// lasers_update_.clear();
std::cout << "2" << std::endl;
map_t* map = map_alloc();
// ROS_ASSERT(map);
std::cout << "3" << std::endl;
map->size_x = map_msg.info.width;
map->size_y = map_msg.info.height;
map->scale = map_msg.info.resolution;
map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
// Convert to player format
std::cout << "4" << std::endl;
map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
//ROS_ASSERT(map->cells);
std::cout << "5" << std::endl;
for(int i=0; i<map->size_x * map->size_y; i++) {
if(map_msg.data[i] == 0)
map->cells[i].occ_state = -1;
else if(map_msg.data[i] == 100)
map->cells[i].occ_state = +1;
else
map->cells[i].occ_state = 0;
}
std::cout << "6" << std::endl;
first_map_received_ = true;
/*
* Initialize the particle filter
*/
#if NEW_UNIFORM_SAMPLING
// Index of free space
std::cout << "7" << std::endl;
free_space_indices.resize(0);
std::cout << "8" << std::endl;
for(int i = 0; i < map_->size_x; i++)
for(int j = 0; j < map_->size_y; j++)
if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
free_space_indices.push_back(std::make_pair(i,j));
#endif
std::cout << "9" << std::endl;
// boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
(pf_init_model_fn_t)AMCLocalizer::uniformPoseGenerator,
(void *)map_);
std::cout << "10" << std::endl;
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;
/*
* Instantiate the sensor objects: Odometry
*/
delete odom_;
odom_ = new AMCLOdom();
// ROS_ASSERT(odom_);
if(odom_model_type_ == ODOM_MODEL_OMNI)
odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
else
odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
/*
* Instantiate the sensor objects: Laser
*/
delete laser_;
laser_ = new AMCLLaser(max_beams_, map_);
// ROS_ASSERT(laser_);
if(laser_model_type_ == LASER_MODEL_BEAM)
laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
sigma_hit_, lambda_short_, 0.0);
else {
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
laser_likelihood_max_dist_);
}
// In case the initial pose message arrived before the first map,
// try to apply the initial pose now that the map has arrived.
applyInitialPose();
}
int
generic_test (const struct test_case *tests,
unsigned ntests,
size_t data_size,
int (ASN1CALL *encode)(unsigned char *, size_t, void *, size_t *),
int (ASN1CALL *length)(void *),
int (ASN1CALL *decode)(unsigned char *, size_t, void *, size_t *),
int (ASN1CALL *free_data)(void *),
int (*cmp)(void *a, void *b),
int (ASN1CALL *copy)(const void *from, void *to))
{
unsigned char *buf, *buf2;
int i;
int failures = 0;
void *data;
struct map_page *data_map, *buf_map, *buf2_map;
#ifdef HAVE_SIGACTION
struct sigaction sa, osa;
#endif
for (i = 0; i < ntests; ++i) {
int ret;
size_t sz, consumed_sz, length_sz, buf_sz;
void *to = NULL;
current_test = tests[i].name;
current_state = "init";
#ifdef HAVE_SIGACTION
sigemptyset (&sa.sa_mask);
sa.sa_flags = 0;
#ifdef SA_RESETHAND
sa.sa_flags |= SA_RESETHAND;
#endif
sa.sa_handler = segv_handler;
sigaction (SIGSEGV, &sa, &osa);
#endif
data = map_alloc(OVERRUN, NULL, data_size, &data_map);
buf_sz = tests[i].byte_len;
buf = map_alloc(UNDERRUN, NULL, buf_sz, &buf_map);
current_state = "encode";
ret = (*encode) (buf + buf_sz - 1, buf_sz,
tests[i].val, &sz);
if (ret != 0) {
printf ("encoding of %s failed %d\n", tests[i].name, ret);
++failures;
continue;
}
if (sz != tests[i].byte_len) {
printf ("encoding of %s has wrong len (%lu != %lu)\n",
tests[i].name,
(unsigned long)sz, (unsigned long)tests[i].byte_len);
++failures;
continue;
}
current_state = "length";
length_sz = (*length) (tests[i].val);
if (sz != length_sz) {
printf ("length for %s is bad (%lu != %lu)\n",
tests[i].name, (unsigned long)length_sz, (unsigned long)sz);
++failures;
continue;
}
current_state = "memcmp";
if (memcmp (buf, tests[i].bytes, tests[i].byte_len) != 0) {
printf ("encoding of %s has bad bytes:\n"
"correct: ", tests[i].name);
print_bytes ((unsigned char *)tests[i].bytes, tests[i].byte_len);
printf ("\nactual: ");
print_bytes (buf, sz);
printf ("\n");
#if 0
rk_dumpdata("correct", tests[i].bytes, tests[i].byte_len);
rk_dumpdata("actual", buf, sz);
exit (1);
#endif
++failures;
continue;
}
buf2 = map_alloc(OVERRUN, buf, sz, &buf2_map);
current_state = "decode";
ret = (*decode) (buf2, sz, data, &consumed_sz);
if (ret != 0) {
printf ("decoding of %s failed %d\n", tests[i].name, ret);
++failures;
continue;
}
if (sz != consumed_sz) {
printf ("different length decoding %s (%ld != %ld)\n",
tests[i].name,
(unsigned long)sz, (unsigned long)consumed_sz);
++failures;
continue;
}
current_state = "cmp";
if ((*cmp)(data, tests[i].val) != 0) {
printf ("%s: comparison failed\n", tests[i].name);
++failures;
continue;
}
current_state = "copy";
if (copy) {
to = emalloc(data_size);
ret = (*copy)(data, to);
if (ret != 0) {
printf ("copy of %s failed %d\n", tests[i].name, ret);
++failures;
continue;
}
current_state = "cmp-copy";
if ((*cmp)(data, to) != 0) {
printf ("%s: copy comparison failed\n", tests[i].name);
++failures;
continue;
}
}
current_state = "free";
if (free_data) {
(*free_data)(data);
if (to) {
(*free_data)(to);
free(to);
}
}
current_state = "free";
map_free(buf_map, tests[i].name, "encode");
map_free(buf2_map, tests[i].name, "decode");
map_free(data_map, tests[i].name, "data");
#ifdef HAVE_SIGACTION
sigaction (SIGSEGV, &osa, NULL);
#endif
}
current_state = "done";
return failures;
}
static void
add(int fd)
{
map_t *gpt, *tpg;
map_t *tbl, *lbt;
map_t *map;
struct gpt_hdr *hdr;
struct gpt_ent *ent;
unsigned int i;
gpt = map_find(MAP_TYPE_PRI_GPT_HDR);
if (gpt == NULL) {
warnx("%s: error: no primary GPT header; run create or recover",
device_name);
return;
}
tpg = map_find(MAP_TYPE_SEC_GPT_HDR);
if (tpg == NULL) {
warnx("%s: error: no secondary GPT header; run recover",
device_name);
return;
}
tbl = map_find(MAP_TYPE_PRI_GPT_TBL);
lbt = map_find(MAP_TYPE_SEC_GPT_TBL);
if (tbl == NULL || lbt == NULL) {
warnx("%s: error: run recover -- trust me", device_name);
return;
}
hdr = gpt->map_data;
if (entry != NOENTRY && entry > le32toh(hdr->hdr_entries)) {
warnx("%s: error: index %u out of range (%u max)", device_name,
entry, le32toh(hdr->hdr_entries));
return;
}
if (entry != NOENTRY) {
i = entry;
ent = (void*)((char*)tbl->map_data + i *
le32toh(hdr->hdr_entsz));
if (!uuid_is_nil(&ent->ent_type, NULL)) {
warnx("%s: error: entry at index %u is not free",
device_name, entry);
return;
}
} else {
/* Find empty slot in GPT table. */
ent = NULL;
for (i = 0; i < le32toh(hdr->hdr_entries); i++) {
ent = (void*)((char*)tbl->map_data + i *
le32toh(hdr->hdr_entsz));
if (uuid_is_nil(&ent->ent_type, NULL))
break;
}
if (i == le32toh(hdr->hdr_entries)) {
warnx("%s: error: no available table entries",
device_name);
return;
}
}
map = map_alloc(block, size);
if (map == NULL) {
warnx("%s: error: no space available on device", device_name);
return;
}
le_uuid_enc(&ent->ent_type, &type);
ent->ent_lba_start = htole64(map->map_start);
ent->ent_lba_end = htole64(map->map_start + map->map_size - 1LL);
hdr->hdr_crc_table = htole32(crc32(tbl->map_data,
le32toh(hdr->hdr_entries) * le32toh(hdr->hdr_entsz)));
hdr->hdr_crc_self = 0;
hdr->hdr_crc_self = htole32(crc32(hdr, le32toh(hdr->hdr_size)));
gpt_write(fd, gpt);
gpt_write(fd, tbl);
hdr = tpg->map_data;
ent = (void*)((char*)lbt->map_data + i * le32toh(hdr->hdr_entsz));
le_uuid_enc(&ent->ent_type, &type);
ent->ent_lba_start = htole64(map->map_start);
ent->ent_lba_end = htole64(map->map_start + map->map_size - 1LL);
hdr->hdr_crc_table = htole32(crc32(lbt->map_data,
le32toh(hdr->hdr_entries) * le32toh(hdr->hdr_entsz)));
hdr->hdr_crc_self = 0;
hdr->hdr_crc_self = htole32(crc32(hdr, le32toh(hdr->hdr_size)));
gpt_write(fd, lbt);
gpt_write(fd, tpg);
printf("%ss%u added\n", device_name, i);
}
void acc_sqlite_init() {
if (acc_sqlite == NULL) {
acc_sqlite = malloc(sizeof(struct acc_sqlite_t_));
acc_sqlite->db_files_map = map_alloc(64, sizeof(sqlite3 *), sizeof(char *), NULL);
}
}