int main(int argc, char **argv)
{
int len, ret=EXIT_FAILURE;
ubx_node_info_t ni;
ubx_block_t *webif;
ubx_data_t *d;
/* initalize the node */
ubx_node_init(&ni, "c-only");
/* load the standard types */
if(ubx_module_load(&ni, "std_types/stdtypes/stdtypes.so") != 0)
goto out;
/* load the web-interface block */
if(ubx_module_load(&ni, "std_blocks/webif/webif.so") != 0)
goto out;
/* create a webserver block */
if((webif = ubx_block_create(&ni, "webif/webif", "webif1"))==NULL)
goto out;
/* Configure port of webserver block
* this gets the ubx_data_t pointer */
d = ubx_config_get_data(webif, "port");
len = strlen(WEBIF_PORT)+1;
/* resize the char array as necessary and copy the port string */
ubx_data_resize(d, len);
strncpy(d->data, WEBIF_PORT, len);
/* init and start the block */
if(ubx_block_init(webif) != 0) {
ERR("failed to init webif");
goto out;
}
if(ubx_block_start(webif) != 0) {
ERR("failed to start webif");
goto out;
}
printf("webif block lauched on port %s, hit enter to quit\n", WEBIF_PORT);
getchar();
ret=EXIT_SUCCESS;
out:
/* this cleans up all blocks and unloads all modules */
ubx_node_cleanup(&ni);
exit(ret);
}
void WorldModel::initializeMicroblx() {
#ifdef BRICS_MICROBLX_ENABLE
this->microBlxPath = MICROBLX_ROOT_DIR;
/* init microblx */
microBlxNodeHandle = new ubx_node_info_t(); // holds all microblx
std::string microBlxNodeName = "functionBlocks";
LOG(DEBUG) << "WorldModel::initializeMicroblx handle with name " << microBlxNodeName << " created.";
if (ubx_node_init(microBlxNodeHandle, microBlxNodeName.c_str()) != 0 ) { //segfaults if started from ubx lua
LOG(ERROR) << "WorldModel::initialize: Cannot initialize the microblx node handle.";
microBlxNodeHandle = 0;
}
LOG(DEBUG) << "WorldModel::initializeMicroblx ubx node created.";
/* load the standard types */
std::string moduleFile = microBlxPath + "std_types/stdtypes/stdtypes.so";
if(ubx_module_load(microBlxNodeHandle, moduleFile.c_str()) != 0){
LOG(ERROR) << "WorldModel::initialize: Cannot load the stdtypes.";
}
/* load the standard types */
moduleFile = "/home/sblume/sandbox/brics_3d_function_blocks/lib/rsg_types.so";
if(ubx_module_load(microBlxNodeHandle, moduleFile.c_str()) != 0){
LOG(ERROR) << "WorldModel::initialize: Cannot load the rsgtypes.";
}
/* load a standard interaction block for sending data */
moduleFile = microBlxPath + "std_blocks/lfds_buffers/lfds_cyclic.so";
if(ubx_module_load(microBlxNodeHandle, moduleFile.c_str()) != 0){
LOG(ERROR) << "WorldModel::initialize: Cannot load the lfds_buffer.";
}
/*
* Create a single (dummy) fifo interaction block to be used
* as default input for any function block
*/
/* create the input fifo block */
std::string name = "inputFifo";
std::string inputModuleName = "lfds_buffers/cyclic";
if((inputBlock = ubx_block_create(microBlxNodeHandle, inputModuleName.c_str(), name.c_str())) == 0){
LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot create the module "<< name;
return;
}
LOG(DEBUG) << "WorldModel::initialize: Created block with name = " << inputBlock->name;
/* configure the fifo block */
// { .name="type_name", .type_name = "char", .doc="name of registered microblx type to transport" },
// { .name="data_len", .type_name = "uint32_t", .doc="array length (multiplier) of data (default: 1)" },
// { .name="buffer_len", .type_name = "uint32_t", .doc="max. number of data elements the buffer shall hold" },
uint32_t dataSize = sizeof(struct rsg_ids); // sizeof(uint32_t); // sizeof(struct rsg_ids);
uint32_t bufferSize = 4;
ubx_data_t* fifoData = ubx_config_get_data(inputBlock, "data_len");
memcpy(fifoData->data, &dataSize, sizeof(dataSize));
fifoData = ubx_config_get_data(inputBlock, "buffer_len");
memcpy(fifoData->data, &bufferSize, sizeof(bufferSize));
fifoData = ubx_config_get_data(inputBlock, "type_name");
int len = strlen("struct rsg_ids")+1;
ubx_data_resize(fifoData, len);
strncpy((char*)fifoData->data, "struct rsg_ids", len);
/* initialize the block */
if(ubx_block_init(inputBlock) != 0){
LOG(ERROR) << "WorldModel::initialize: Cannot initialize the module "<< name;
return;
}
/* start the block */
if(ubx_block_start(inputBlock) != 0){
LOG(ERROR) << "WorldModel::initialize: Cannot start the module "<< name;
return;
}
/*
* Create a second (dummy) fifo interaction block to be used
* as default output for any function block
*/
/* create the output fifo block */
name = "outputFifo";
inputModuleName = "lfds_buffers/cyclic";
if((outputBlock = ubx_block_create(microBlxNodeHandle, inputModuleName.c_str(), name.c_str())) == 0){
LOG(ERROR) << "WorldModel::loadFunctionBlock: Cannot create the module "<< name;
return;
}
LOG(DEBUG) << "WorldModel::initialize: Created block with name = " << outputBlock->name;
dataSize = sizeof(struct rsg_ids);
bufferSize = 4;
ubx_data_t* outputFifoData = ubx_config_get_data(outputBlock, "data_len");
memcpy(outputFifoData->data, &dataSize, sizeof(dataSize));
outputFifoData = ubx_config_get_data(outputBlock, "buffer_len");
memcpy(outputFifoData->data, &bufferSize, sizeof(bufferSize));
outputFifoData = ubx_config_get_data(outputBlock, "type_name");
len = strlen("struct rsg_ids")+1; //'rsg_ids' but should be 'struct rsg_ids'
ubx_data_resize(outputFifoData, len);
strncpy((char*)outputFifoData->data, "struct rsg_ids", len);
/* initialize the block */
if(ubx_block_init(outputBlock) != 0){
LOG(ERROR) << "WorldModel::initialize: Cannot initialize the module "<< name;
return;
}
/* start the block */
if(ubx_block_start(outputBlock) != 0){
LOG(ERROR) << "WorldModel::initialize: Cannot start the module "<< name;
return;
}
outputBlockCopy = outputBlock;
// WorldModel::microBlxWmHandle = this;
#endif
}
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;
}
