/* start */ int random_start(ubx_block_t *b) { DBG("in"); uint32_t seed, ret; unsigned int clen; struct random_config* rndconf; struct rnd_info* inf; inf=(struct rnd_info*) b->private_data; /* get and store min_max_config */ rndconf = (struct random_config*) ubx_config_get_data_ptr(b, "min_max_config", &clen); inf->info.min = rndconf->min; inf->info.max = (rndconf->max == 0) ? INT_MAX : rndconf->max; /* seed is allowed to change at runtime, check if new one available */ ubx_port_t* seed_port = ubx_port_get(b, "seed"); ret = read_seed(seed_port, &seed); if(ret>0) { DBG("starting component. Using seed: %d, min: %d, max: %d", seed, inf->info.min, inf->info.max); srandom(seed); } else { DBG("starting component. Using min: %d, max: %d", inf->info.min, inf->info.max); } return ret; }
/** * luablock_step - execute lua string and call step hook * * @param b */ static void luablock_step(ubx_block_t *b) { int len=0, ret; struct luablock_info* inf = (struct luablock_info*) b->private_data; /* any lua code to execute */ ubx_port_t* p_exec_str = ubx_port_get(b, "exec_str"); if((len=__port_read(p_exec_str, inf->exec_str_buff))>0) { if ((ret=luaL_dostring(inf->L, inf->exec_str_buff->data)) != 0) { ERR("Failed to exec_str: %s", lua_tostring(inf->L, -1)); goto out; } } call_hook(b, "step", 0, 0); out: /* TODO: fix this. realloc could have changed port addr */ if(len>0) { p_exec_str = ubx_port_get(b, "exec_str"); write_int(p_exec_str, &ret); } return; }
/* step */ void random_step(ubx_block_t *b) { unsigned int rand_val; struct rnd_info* inf; inf=(struct rnd_info*) b->private_data; ubx_port_t* rand_port = ubx_port_get(b, "rnd"); rand_val = random(); rand_val = (rand_val > inf->info.max) ? (rand_val%inf->info.max) : rand_val; rand_val = (rand_val < inf->info.min) ? ((inf->info.min + rand_val)%inf->info.max) : rand_val; write_rnd(rand_port, &rand_val); }
static int youbot_start(ubx_block_t *b) { DBG(" "); int ret=-1, i; struct youbot_info *inf=b->private_data; /* reset old commands */ if(inf->base.detected && base_prepare_start(&inf->base) != 0) goto out; if(inf->arm1.detected && arm_prepare_start(&inf->arm1) != 0) goto out; if(inf->arm2.detected && arm_prepare_start(&inf->arm2) != 0) goto out; /* requesting operational for all slaves */ ec_slave[0].state = EC_STATE_OPERATIONAL; ec_writestate(0); /* wait for all slaves to reach SAFE_OP state */ ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); if(ec_slave[0].state != EC_STATE_OPERATIONAL) { ERR("not all slaves reached state operational:"); for (i=0; i<=ec_slavecount; i++) { if (ec_slave[i].state != EC_STATE_OPERATIONAL) { ERR("\tslave %d (%s) is in state=0x%x, ALstatuscode=0x%x", i, ec_slave[i].name, ec_slave[i].state, ec_slave[i].ALstatuscode); } } goto out; } /* get things rolling */ if (ec_send_processdata() <= 0) { inf->pd_send_err++; ERR("sending initial process data failed"); } /* cache port pointers */ assert(inf->base.p_control_mode = ubx_port_get(b, "base_control_mode")); assert(inf->base.p_cmd_twist = ubx_port_get(b, "base_cmd_twist")); assert(inf->base.p_cmd_vel = ubx_port_get(b, "base_cmd_vel")); assert(inf->base.p_cmd_cur = ubx_port_get(b, "base_cmd_cur")); assert(inf->base.p_msr_odom = ubx_port_get(b, "base_msr_odom")); assert(inf->base.p_msr_twist = ubx_port_get(b, "base_msr_twist")); assert(inf->base.p_base_motorinfo = ubx_port_get(b, "base_motorinfo")); assert(inf->arm1.p_control_mode = ubx_port_get(b, "arm1_control_mode")); assert(inf->arm1.p_calibrate_cmd = ubx_port_get(b, "arm1_calibrate_cmd")); assert(inf->arm1.p_cmd_pos = ubx_port_get(b, "arm1_cmd_pos")); assert(inf->arm1.p_cmd_vel = ubx_port_get(b, "arm1_cmd_vel")); assert(inf->arm1.p_cmd_cur = ubx_port_get(b, "arm1_cmd_cur")); assert(inf->arm1.p_cmd_eff = ubx_port_get(b, "arm1_cmd_eff")); assert(inf->arm1.p_arm_state = ubx_port_get(b, "arm1_state")); assert(inf->arm1.p_arm_motorinfo = ubx_port_get(b, "arm1_motorinfo")); assert(inf->arm1.p_gripper = ubx_port_get(b, "arm1_gripper")); ret=0; out: return ret; }
bool WorldModel::executeFunctionBlock(std::string name, std::vector<rsg::Id>& input, std::vector<rsg::Id>& output) { #ifdef BRICS_MICROBLX_ENABLE /* check if node is initialized */ if (microBlxNodeHandle == 0) { LOG(ERROR) << "WorldModel::executeFunctionBlock: microblx node handle not initialized. Cannot execute function block "<< name; return false; } // LOG(DEBUG) << "WorldModel::executeFunctionBlock: The are currently loaded: " << std::endl // << "\t" << ubx_num_blocks(microBlxNodeHandle) << " block(s)" << std::endl // << "\t" << ubx_num_types(microBlxNodeHandle) << " type(s)" << std::endl // << "\t" << ubx_num_modules(microBlxNodeHandle)<< " module(s)"; /* resolve name to block handle */ ubx_block_t* block = ubx_block_get(microBlxNodeHandle, name.c_str()); if (block == 0) { LOG(ERROR) << "WorldModel::executeFunctionBlock: function block " << name << " does not exist."; return false; } /* Translate inputs */ ubx_port_t* inputPort = ubx_port_get(block, "inputDataIds"); ubx_type_t* type = ubx_type_get(microBlxNodeHandle, "struct rsg_ids"); if(inputPort == 0) { LOG(WARNING) << "WorldModel::executeFunctionBlock: function block " << name << " has no inputDataIds port"; // return false; } else { // Only write input if it can be read... /* prepare rsg type */ rsg_ids tmpIds; // ubx_type_t* type = ubx_type_get(microBlxNodeHandle, "struct rsg_ids"); brics_3d::rsg::UbxTypecaster::convertIdsToUbx(input, tmpIds); // rsg -> ubx type (structs) /* stuff everything into generic ubx_data_t struct */ ubx_data_t val; val.type = type; val.len = 1;// because 1* ids struct ... sizeof(tmpIds);//inputPointCloudId); val.data = &tmpIds; /* push data into the port */ inputBlock->write(inputBlock, &val); inputBlock->stat_num_writes++; } /* Execute now! */ if (ubx_cblock_step(block) != 0) { LOG(ERROR) << "WorldModel::executeFunctionBlock: cannot execute function block " << name; return false; } /* Translate outputs */ output.clear(); ubx_port_t* outputPort = ubx_port_get(block, "outputDataIds"); if(outputPort == 0) { LOG(WARNING) << "WorldModel::executeFunctionBlock: function block " << name << " has no outputDataIds port"; } else { rsg_ids recievedOutputDataIds; recievedOutputDataIds.numberOfIds = 0u; // int ret = read_rsg_ids(outputPort, &recievedOutputDataIds); // if (ret < 1) { // LOG(WARNING) << "WorldModel::executeFunctionBlock: No output IDs given."; // } ubx_data_t val; val.type = type; val.len = 1;// because 1* ids struct ... sizeof(tmpIds);//inputPointCloudId); val.data = &recievedOutputDataIds; if (outputBlock != outputBlockCopy) { //workaround LOG(ERROR) << "Handle to output block has been corrupted." << std::endl; return false; } outputBlock->read(outputBlock, &val); //can segfault outputBlock->stat_num_reads++; brics_3d::rsg::UbxTypecaster::convertIdsFromUbx(recievedOutputDataIds, output); } return true; #else blockIterator = loadedFunctionBlocks.find(name); if (blockIterator != loadedFunctionBlocks.end()) { LOG(DEBUG) << "WorldModel::executeFunctionBlock: executing block " << name << "."; if (blockIterator->second.functionBlock != 0) { /* Get the block */ FunctionBlockModuleInfo module = blockIterator->second; IFunctionBlock* block = FunctionBlockLoader::moduleToBlock(module); /* Set data, execute and write pack the results */ return block->execute(input, output); } else { LOG(ERROR) << "WorldModel::executeFunctionBlock: Loaded block is invalid. Aborting execution."; return false; } } else { LOG(WARNING) << "WorldModel::executeFunctionBlock: can not find block " << name << " because it is is not yet loaded. Skipping attempt to execute it."; return false; } return true; #endif // LOG(ERROR) << "Microblx support not enabled. Cannot load a function block."; // return false; }
bool WorldModel::loadFunctionBlock(std::string name, std::string path) { #ifdef BRICS_MICROBLX_ENABLE /* initialize on first load * This is basically a workaround for the segfault caused by crweatin a world model * via the lua bindings. In Lua you cannot call loadFunctionBlock right now... */ if (!microBloxIsInitialized) { initializeMicroblx(); microBloxIsInitialized = true; } std::string moduleFile; ubx_block_t* block; LOG(DEBUG) << "WorldModel::loadFunctionBlock: Loading a new function block to the world model with name " << name; /* In a nutshell: you'll need to dlopen(1) the * block or type library and register it with a node, the create the block * instances, configure and start them. */ /* check if node is initialized */ if (microBlxNodeHandle == 0) { LOG(ERROR) << "WorldModel::loadFunctionBlock: microblx node handle not initialized. Cannot load function block "<< name; return false; } /* load the block */ moduleFile = path + name + ".so"; if(ubx_module_load(microBlxNodeHandle, moduleFile.c_str()) != 0) { LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot load the module "<< name; return false; } /* create the block */ std::string blockName = name + "/" + name; // e.g. "cppdemo/cppdemo" if((block = ubx_block_create(microBlxNodeHandle, blockName.c_str(), name.c_str())) == 0){ LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot create the module "<< name; return false; } LOG(DEBUG) << "WorldModel::loadFunctionBlock: Created block with name = " << block->name; /* configure the block - i.e. pass the world model handle */ //{ .name="wm_handle", .type_name = "struct rsg_wm_handle", doc="Handle to the world wodel instance. This parameter is mandatory."}, ubx_data_t* configData = ubx_config_get_data(block, "wm_handle"); if (configData != 0) { rsg_wm_handle tmpUbxWorldModleHandle; // Ubx and Lua parsable verison of a world model handle. tmpUbxWorldModleHandle.wm = reinterpret_cast<void*>(this); memcpy(configData->data, &tmpUbxWorldModleHandle, sizeof(tmpUbxWorldModleHandle)); } else { LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot configure the module "<< name << ", because its configuration paremeter with name wmHandle is missing."; return false; } /* initialize the block */ if(ubx_block_init(block) != 0){ LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot initialize the module "<< name; return false; } /* start the block */ if(ubx_block_start(block) != 0){ LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot start the module "<< name; return false; } ubx_port_t* port = ubx_port_get(block, "inputDataIds"); if(port == 0) { LOG(WARNING) << "WorldModel::loadFunctionBlock: function block " << name << " has no inputDataIds port"; // return false ? } else { /* connect to default input */ if(ubx_port_connect_in(port, inputBlock)) { LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot connect the module "<< name << "to the default input port."; return false; } } ubx_port_t* outputPort = ubx_port_get(block, "outputDataIds"); if(outputPort == 0) { LOG(WARNING) << "WorldModel::loadFunctionBlock: function block " << name << " has no outputDataIds port"; // return false ? } else { /* connect to default input */ if(ubx_port_connect_out(outputPort, outputBlock)) { LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot connect the module "<< name << "to the default output port."; return false; } } LOG(DEBUG) << "WorldModel::loadFunctionBlock: The are currently loaded: " << std::endl << "\t" << ubx_num_blocks(microBlxNodeHandle) << " block(s)" << std::endl << "\t" << ubx_num_types(microBlxNodeHandle) << " type(s)" << std::endl << "\t" << ubx_num_modules(microBlxNodeHandle)<< " module(s)"; LOG(INFO) << "WorldModel::loadFunctionBlock: function block " << name << " has been successfully loaded."; return true; #else blockIterator = loadedFunctionBlocks.find(name); if (blockIterator == loadedFunctionBlocks.end()) { // does not exists yet LOG(DEBUG) << "WorldModel::loadFunctionBlock: block " << name << " not loaded yet. Trying to load it now."; FunctionBlockLoader loader; FunctionBlockModuleInfo block; if(loader.loadFunctionBlock(name, path, this, block)) { loadedFunctionBlocks.insert(std::make_pair(name, block)); } else { LOG(ERROR) << "WorldModel::loadFunctionBlock: can not load a function " << name << " block via FunctionBlockLoader"; return false; } } else { // block exist already LOG(WARNING) << "WorldModel::loadFunctionBlock: block " << name << " already loaded yet. Skipping attempt to load it."; // We don't return false, as a second attempt to load is not an error. This might happen if multiple modules load the // same block. } #endif // LOG(ERROR) << "Microblx support not enabled. Cannot load a function block."; // return false; return true; }
/* declare a helper function to update the port cache this is necessary * because the port ptrs can change if ports are dynamically added or * removed. This function should hence be called after all * initialization is done, i.e. typically in 'start' */ static void update_port_cache(ubx_block_t *b, struct zmq_server_port_cache *pc) { pc->zmq_req = ubx_port_get(b, "zmq_req"); pc->zmq_rep = ubx_port_get(b,"zmq_rep"); }
/* declare a helper function to update the port cache this is necessary * because the port ptrs can change if ports are dynamically added or * removed. This function should hence be called after all * initialization is done, i.e. typically in 'start' */ static void update_port_cache(ubx_block_t *b, struct rsg_dump_port_cache *pc) { pc->rsg_out = ubx_port_get(b, "rsg_out"); }
/* declare a helper function to update the port cache this is necessary * because the port ptrs can change if ports are dynamically added or * removed. This function should hence be called after all * initialization is done, i.e. typically in 'start' */ static void update_port_cache(ubx_block_t *b, struct rsg_json_reciever_port_cache *pc) { pc->rsg_in = ubx_port_get(b, "rsg_in"); }