int handle_event(zloop_t *loop, zsock_t *reader, void *args) {
// initialization
ubx_block_t *b = (ubx_block_t *) args;
struct zmq_receiver_info *inf = (struct zmq_receiver_info*) b->private_data;
printf("zmq_receiver: data available.\n");
zframe_t *frame = zframe_recv (reader);
// print out frame data
zframe_print (frame, NULL);
// move to step function?
ubx_type_t* type = ubx_type_get(b->ni, "unsigned char");
ubx_data_t msg;
msg.data = (void *)zframe_data(frame);
msg.len = zframe_size(frame);
msg.type = type;
//hexdump((unsigned char *)msg.data, msg.len, 16);
__port_write(inf->ports.zmq_in, &msg);
/* Inform potential observers ? */
// clean up temporary frame object
zframe_destroy (&frame);
return 1;
}
/* step */
void sherpa_msg_step(ubx_block_t *b)
{
struct sherpa_msg_info *inf = (struct sherpa_msg_info*) b->private_data;
// std::cout << "sherpa_msg: Processing a port update" << std::endl;
/* Read data from port */
ubx_port_t* port = inf->ports.msg_in;
assert(port != 0);
ubx_data_t msg;
checktype(port->block->ni, port->in_type, "unsigned char", port->name, 1);
msg.type = port->in_type;
msg.len = inf->buffer_length;
msg.data = inf->buffer;
// std::cout << "sherpa_msg: Reading from port" << std::endl;
int read_bytes = __port_read(port, &msg);
if (read_bytes <= 0) {
// std::cout << "sherpa_msg: No data recieved from port" << std::endl;
return;
}
std::cout << "sherpa_msg: read bytes = " << read_bytes << std::endl;
/* Either decode or encode message */
/* Write result back t0 port */
ubx_type_t* type = ubx_type_get(b->ni, "unsigned char");
ubx_data_t result_msg;
// result_msg.data = (void *)zframe_data(frame);
// result_msg.len = zframe_size(frame);
result_msg.type = type;
//hexdump((unsigned char *)result_msg.data, result_msg.len, 16);
__port_write(inf->ports.msg_out, &result_msg);
}
void* recieverThread(void* arg) {
ubx_block_t *b = (ubx_block_t *) arg;
struct zmq_reciever_info *inf = (struct zmq_reciever_info*) b->private_data;
std::cout << " recieverThread started." << std::endl;
/* Receiver loop */
while(true) {
zmq::message_t update;
inf->subscriber->recv(&update);
std::cout << "zmq_reciever: Recieved " << update.size() << " bytes from ?" << std::endl;
if (update.size() < 1) {
std::cout << "recv()" << std::endl;
break;
}
// move to step function?
ubx_type_t* type = ubx_type_get(b->ni, "unsigned char");
ubx_data_t msg;
msg.data = (void *)update.data();
msg.len = update.size();
msg.type = type;
//hexdump((unsigned char *)msg.data, msg.len, 16);
__port_write(inf->ports.zmq_in, &msg);
/* Inform potential observers ? */
}
/* Clean up */
return 0;
}
static void
zyre_bridge_actor (zsock_t *pipe, void *args)
{
// initialization
ubx_block_t *b = (ubx_block_t *) args;
struct zyre_bridge_info *inf = (struct zyre_bridge_info*) b->private_data;
printf("[zyre_bridge]: actor started.\n");
// send signal on pipe socket to acknowledge initialization
zsock_signal (pipe, 0);
bool terminated = false;
zpoller_t *poller = zpoller_new (pipe, zyre_socket (inf->node), NULL);
while (!terminated) {
void *which = zpoller_wait (poller, -1);
// handle msgs from main thread
if (which == pipe) {
zmsg_t *msg = zmsg_recv (which);
if (!msg)
break; // Interrupted
// only react to TERM signal
char *command = zmsg_popstr (msg);
if (streq (command, "$TERM"))
terminated = true;
else {
puts ("Invalid pipe message to actor");
}
free (command);
zmsg_destroy (&msg);
} else
// handle msgs from zyre network
if (which == zyre_socket (inf->node)) {
zmsg_t *msg = zmsg_recv (which);
if (!msg) {
printf("[zyre_bridge]: interrupted!\n");
}
char *event = zmsg_popstr (msg);
char *peer = zmsg_popstr (msg);
char *name = zmsg_popstr (msg);
if (streq (event, "ENTER"))
printf ("[zyre_bridge]: %s has entered\n", name);
else
if (streq (event, "EXIT"))
printf ("[zyre_bridge]: %s has exited\n", name);
else
if (streq (event, "SHOUT")) {
printf ("[zyre_bridge]: SHOUT received from %s.\n", name);
char *group = zmsg_popstr (msg);
char *message = zmsg_popstr (msg);
// load JSON msg
json_t *m;
json_error_t error;
m= json_loads(message,0,&error);
if(!m) {
printf("Error parsing JSON payload! line %d: %s\n", error.line, error.text);
json_decref(m);
return;
}
printf("%s\n",message);
if (json_object_get(m, "type")) {
std::string type = json_dumps(json_object_get(m, "type"), JSON_ENCODE_ANY);
type = type.substr(1, type.size()-2); // get rid of " characters
printf("type: %s\n",json_dumps(json_object_get(m, "type"), JSON_ENCODE_ANY));
for (int i=0; i < inf->input_type_list.size();i++)
{
//if (json_string_value(json_object_get(m, "type")) == inf->input_type_list[i]){
//printf("type list, type : %s, %s \n", inf->input_type_list[i].c_str(), type.c_str());
if (inf->input_type_list[i].compare(type) == 0) {
ubx_type_t* type = ubx_type_get(b->ni, "unsigned char");
ubx_data_t ubx_msg;
ubx_msg.data = (void *)json_dumps(json_object_get(m, "payload"), JSON_ENCODE_ANY);
printf("message: %s\n",json_dumps(json_object_get(m, "payload"), JSON_ENCODE_ANY));
ubx_msg.len = strlen(json_dumps(json_object_get(m, "payload"), JSON_ENCODE_ANY));
ubx_msg.type = type;
__port_write(inf->ports.zyre_in, &ubx_msg);
}
}
} else {
printf("Error parsing JSON string! Does not conform to msg model.\n");
}
free (group);
free (message);
}
else
if (streq (event, "WHISPER")){
char *message = zmsg_popstr (msg);
printf ("%s: WHISPER \n%s\n", name, message);
free (message);
}
else
if (streq (event, "EVASIVE"))
printf ("[zyre_bridge]: %s is being evasive\n", name);
free (event);
free (peer);
free (name);
zmsg_destroy (&msg);
}
}
zpoller_destroy (&poller);
//TODO: make parametrizable
zclock_sleep (100);
}
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;
}
// .split main task
// We have a single task that implements the worker side of the
// Paranoid Pirate Protocol (PPP). The interesting parts here are
// the heartbeating, which lets the worker detect if the queue has
// died, and vice versa:
static void ppworker_actor(zsock_t *pipe, void *args)
{
ubx_block_t *b = (ubx_block_t *) args;
struct czmq_ppworker_info *inf = (struct czmq_ppworker_info*) b->private_data;
zsock_t *worker = s_worker_socket ();
// If liveness hits zero, queue is considered disconnected
size_t liveness = HEARTBEAT_LIVENESS;
size_t interval = INTERVAL_INIT;
// Send out heartbeats at regular intervals
uint64_t heartbeat_at = zclock_time () + HEARTBEAT_INTERVAL;
srandom ((unsigned) time (NULL));
printf("ppworker: actor started.\n");
// send signal on pipe socket to acknowledge initialisation
zsock_signal (pipe, 0);
while (true) {
zmq_pollitem_t items [] = { { zsock_resolve(worker), 0, ZMQ_POLLIN, 0 } };
int rc = zmq_poll (items, 1, HEARTBEAT_INTERVAL * ZMQ_POLL_MSEC);
if (rc == -1)
break; // Interrupted
if (items [0].revents & ZMQ_POLLIN) {
// Get message
// - 3-part envelope + content -> request
// - 1-part HEARTBEAT -> heartbeat
zmsg_t *msg = zmsg_recv (worker);
if (!msg)
break; // Interrupted
if (zmsg_size (msg) == 3) {
printf ("I: normal reply\n");
byte *buffer;
size_t buffer_size = zmsg_encode (msg, &buffer);
ubx_type_t* type = ubx_type_get(b->ni, "unsigned char");
ubx_data_t umsg;
umsg.data = (void *)buffer;
umsg.len = buffer_size;
umsg.type = type;
__port_write(inf->ports.zmq_in, &umsg);
zmsg_send (&msg, worker);
liveness = HEARTBEAT_LIVENESS;
sleep (1); // Do some heavy work
if (zsys_interrupted)
break;
}
else
// .split handle heartbeats
// When we get a heartbeat message from the queue, it means the
// queue was (recently) alive, so we must reset our liveness
// indicator:
if (zmsg_size (msg) == 1) {
zframe_t *frame = zmsg_first (msg);
if (memcmp (zframe_data (frame), PPP_HEARTBEAT, 1) == 0)
liveness = HEARTBEAT_LIVENESS;
else {
printf ("E: invalid message\n");
zmsg_dump (msg);
}
zmsg_destroy (&msg);
}
else {
printf ("E: invalid message\n");
zmsg_dump (msg);
}
interval = INTERVAL_INIT;
}
else
// .split detecting a dead queue
// If the queue hasn't sent us heartbeats in a while, destroy the
// socket and reconnect. This is the simplest most brutal way of
// discarding any messages we might have sent in the meantime:
if (--liveness == 0) {
printf ("W: heartbeat failure, can't reach queue\n");
printf ("W: reconnecting in %zd msec...\n", interval);
zclock_sleep (interval);
if (interval < INTERVAL_MAX)
interval *= 2;
zsock_destroy(&worker);
worker = s_worker_socket ();
liveness = HEARTBEAT_LIVENESS;
}
// Send heartbeat to queue if it's time
if (zclock_time () > heartbeat_at) {
heartbeat_at = zclock_time () + HEARTBEAT_INTERVAL;
printf ("I: worker heartbeat\n");
zframe_t *frame = zframe_new (PPP_HEARTBEAT, 1);
zframe_send (&frame, worker, 0);
}
}
}
