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);
}
/* init */
static int fifo_init(ubx_block_t *i)
{
int ret = -1;
struct fifo_block_info* bbi;
if((i->private_data = calloc(1, sizeof(struct fifo_block_info)))==NULL) {
ERR("failed to alloc fifo_block_info");
goto out;
}
bbi = (struct fifo_block_info*) i->private_data;
pthread_mutex_init(&bbi->mutex, NULL);
bbi->size = *((uint32_t*) ubx_config_get_data(i, "fifo_size"));
if(bbi->size==0) {
/* goto out; */
bbi->size=16;
ERR("invalid fifosize 0, setting to %ld TODO: FIXME!", bbi->size);
}
if((bbi->buff=malloc(bbi->size))==NULL) {
ERR("failed to allocate fifo");
goto out_free_priv_data;
}
/* set to empty */
bbi->rdptr = bbi->wrptr = bbi->buff;
bbi->type=NULL;
DBG("allocated fifo of size %ld", bbi->size);
ret=0;
goto out;
out_free_priv_data:
free(i->private_data);
out:
return ret;
};
/* init */
int zyre_bridge_init(ubx_block_t *b)
{
int ret = -1;
struct zyre_bridge_info *inf;
unsigned int tmplen;
/* allocate memory for the block local state */
if ((inf = (struct zyre_bridge_info*)calloc(1, sizeof(struct zyre_bridge_info)))==NULL) {
ERR("zyre_bridge: failed to alloc memory");
ret=EOUTOFMEM;
return ret;
}
update_port_cache(b, &inf->ports);
//If the following code is used, do not forget to also use the code freeing the memory in the cleanup. And also to put it into the bridge_info struct
// inf->msg_buffer = (unsigned char*) malloc(inf->max_msg_length*sizeof(unsigned char*));
// if (!inf->msg_buffer){
// printf("%s: Could not allocate memory for msg buffer. \n", b->name);
// goto out;
// }
int *max_msg_length;
max_msg_length = (int*) ubx_config_get_data_ptr(b, "max_msg_length", &tmplen);
printf("max_msg_length value for block %s is %d\n", b->name, *max_msg_length);
inf->max_msg_length = *max_msg_length;
if (inf->max_msg_length <=0){
printf("ERR: %s: max_msg_length must be >0!\n",b->name);
return ret;
}
int *max_send;
max_send = (int*) ubx_config_get_data_ptr(b, "max_send", &tmplen);
printf("max_send value for block %s is %d\n", b->name, *max_send);
inf->max_send = *max_send;
// ///TODO: need to get a list of type names in here
// char *type_list;
// type_list = (char*) ubx_config_get_data_ptr(b, "type_list", &tmplen);
// printf("List of types for block %s is %s\n", b->name, type_list);
// inf->type_list = type_list;
///TODO: for now hardcoded list -> fix, read from comma separated string
inf->input_type_list.push_back("RSGUpdate_global");
inf->input_type_list.push_back("RSGUpdate_local");
inf->input_type_list.push_back("RSGUpdate_both");
inf->input_type_list.push_back("RSGUpdate");
inf->input_type_list.push_back("RSGQuery");
inf->input_type_list.push_back("RSGFunctionBlock");
inf->output_type_list.push_back("RSGUpdate"); //updates generated for updating other RSG agents, will be mapped to RSGUpdate_global
inf->output_type_list.push_back("RSGUpdateResult");
inf->output_type_list.push_back("RSGQueryResult");
inf->output_type_list.push_back("RSGFunctionBlockResult");
int major, minor, patch;
zyre_version (&major, &minor, &patch);
if (major != ZYRE_VERSION_MAJOR)
return ret;
if (minor != ZYRE_VERSION_MINOR)
return ret;
if (patch != ZYRE_VERSION_PATCH)
return ret;
char *wm_name;
wm_name = (char*) ubx_config_get_data_ptr(b, "wm_name", &tmplen);
printf("zyre name for block %s is %s\n", b->name, wm_name);
zyre_t *node;
node = zyre_new (wm_name);
if (!node){
printf("Could not create a zyre node!");
return ret;
}
inf->node = node;
int rc;
//char *loc_ep;
char *gos_ep;
///TODO: remove superfluous local endpoint
//loc_ep = (char*) ubx_config_get_data_ptr(b, "local_endpoint", &tmplen);
gos_ep = (char*) ubx_config_get_data_ptr(b, "gossip_endpoint", &tmplen);
//printf("local and gossip endpoint for block %s is %s and %s\n", b->name, loc_ep, gos_ep);
//rc = zyre_set_endpoint (node, "%s",loc_ep);
//assert (rc == 0);
// check if zyre or gossip shall be used
ubx_data_t *tmp;
tmp = ubx_config_get_data(b, "gossip_flag");
int gossip_flag;
gossip_flag = *(int*) tmp->data;
if (gossip_flag == 1){
// Set up gossip network for this node
ubx_data_t *dmy;
dmy = ubx_config_get_data(b, "bind");
int bind;
bind = *(int*) dmy->data;
if (bind == 1) {
printf("%s: This block will bind to gossip endpoint '%s'\n", b->name,gos_ep);
zyre_gossip_bind (node, "%s", gos_ep);
} else if (bind == 0) {
printf("%s: This block will connect to gossip endpoint '%s' \n", b->name,gos_ep);
zyre_gossip_connect (node, "%s",gos_ep);
} else {
printf("%s: Wrong value for bind configuration. Must be 0 or 1. \n", b->name);
return ret;
}
} else if (gossip_flag == 0) {
//nothing to do here. if no gossip port was set, zyre will use UDP beacons automatically
} else {
printf("%s: Wrong value for gossip_flag configuration. Must be 0 or 1. \n", b->name);
return ret;
}
rc = zyre_start (node);
assert (rc == 0);
///TODO: enable list of group names
char *group;
group = (char*) ubx_config_get_data_ptr(b, "group", &tmplen);
zyre_join (node, group);
inf->group = group;
// Give time for them to interconnect
zclock_sleep (100);
b->private_data=inf;
ret=0;
return ret;
}
/* step */
void zyre_bridge_step(ubx_block_t *b)
{
struct zyre_bridge_info *inf = (struct zyre_bridge_info*) b->private_data;
/* Read data from port */
ubx_port_t* port = inf->ports.zyre_out;
assert(port != 0);
char * tmp_str = (char*) malloc(inf->max_msg_length*sizeof(char*));
ubx_data_t msg;
checktype(port->block->ni, port->in_type, "unsigned char", port->name, 1);
msg.type = port->in_type;
msg.len = inf->max_msg_length;
msg.data = tmp_str;
//msg.data = inf->msg_buffer;
int counter = 0;
while (counter < inf->max_send) {
int read_bytes = __port_read(port, &msg);
//printf("zyrebidge: read bytes: %d\n",read_bytes);
//printf("step: read strlen: %lu\n",strlen((char*) msg.data));
if (read_bytes <= 0) {
//printf("zyre_bridge: No data recieved from port\n");
free(tmp_str);
return;
}
// printf("zyrebridge: read bytes: %d\n",read_bytes);
// port_read returns byte array. Need to add 0 termination manually to the string.
tmp_str[read_bytes] = '\0';
// create json object and...
json_t *pl;
json_error_t error;
pl= json_loads(tmp_str,0,&error);
if(!pl) {
printf("Error parsing JSON payload! line %d: %s\n", error.line, error.text);
json_decref(pl);
free(tmp_str);
return;
}
// printf("[zyrebidge] retrieving msg: %s\n", json_dumps(pl, JSON_ENCODE_ANY));
// ...check for its type and embed it into msg envelope
json_t *new_msg;
new_msg = json_object();
json_object_set(new_msg, "payload", pl);
json_object_set(new_msg, "metamodel", json_string("SHERPA"));
if(json_object_get(pl, "@worldmodeltype") == 0) {
printf("[zyrebidge] retrieving msg: %s\n", json_dumps(pl, JSON_ENCODE_ANY));
printf("[zyrebidge] Error parsing RSG payload! @worldmodeltype is missing.\n");
json_decref(pl);
free(tmp_str);
return;
}
std::string tmp_type = json_string_value(json_object_get(pl, "@worldmodeltype")); //can segfault
char *send_msg;
for (int i=0; i < inf->output_type_list.size();i++)
{
if (tmp_type.compare(inf->output_type_list[i])) {
// need to handle exception for updates generated from RSG due to local updates
if (tmp_type.compare("RSGUpdate") == 0) {
json_object_set(new_msg, "model", json_string("RSGUpdate"));
json_object_set(new_msg, "type", json_string("RSGUpdate_global"));
// If used with mediator, add send_request envelope
ubx_data_t *dmy;
dmy = ubx_config_get_data(b, "mediator");
int mediator;
mediator = *(int*) dmy->data;
if (mediator == 1) {
zuuid_t *query_uuid = zuuid_new ();
assert (query_uuid);
json_t *recip = json_array();
assert((recip)&&(json_array_size(recip)==0));
send_msg = send_request(zuuid_str(query_uuid),zyre_uuid(inf->node),recip,1000,"send_remote",new_msg);
}
else {
send_msg = json_dumps(new_msg, JSON_ENCODE_ANY);
}
} else {
json_object_set(new_msg, "model", json_string(tmp_type.c_str()));
json_object_set(new_msg, "type", json_string(tmp_type.c_str()));
send_msg = json_dumps(new_msg, JSON_ENCODE_ANY);
}
} else {
printf("[zyre_bridge] Unknown output type: %s!\n",tmp_type.c_str());
}
}
printf("[zyrebidge] sending msg: %s\n", send_msg);
zyre_shouts(inf->node, inf->group, "%s", send_msg);
counter++;
json_decref(pl);
json_decref(new_msg);
}
free(tmp_str);
return;
}
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;
}
