/* init */
int zmq_receiver_init(ubx_block_t *b)
{
int ret = -1;
struct zmq_receiver_info *inf;
unsigned int tmplen;
char *connection_spec_str;
// CZMQ socket for subscriber
zsock_t* sub;
/* allocate memory for the block local state */
if ((inf = (struct zmq_receiver_info*)calloc(1, sizeof(struct zmq_receiver_info)))==NULL) {
ERR("zmq_receiver: failed to alloc memory");
ret=EOUTOFMEM;
goto out;
}
b->private_data=inf;
update_port_cache(b, &inf->ports);
connection_spec_str = (char*) ubx_config_get_data_ptr(b, "connection_spec", &tmplen);
printf("ZMQ connection configuration for block %s is %s\n", b->name, connection_spec_str);
// create subscriber socket and subscribe to all messages
sub = zsock_new_sub(connection_spec_str, "");
zsock_set_subscribe(sub, "");
if (!sub)
goto out;
// add pointer to subscriber to private data
inf->subscriber = sub;
ret=0;
out:
return ret;
}
/* 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;
}
static int luablock_init(ubx_block_t *b)
{
char *lua_file, *lua_str;
unsigned int lua_file_len, lua_str_len;
int ret = -EOUTOFMEM;
struct luablock_info* inf;
if((inf = calloc(1, sizeof(struct luablock_info)))==NULL)
goto out;
b->private_data = inf;
lua_file = (char *) ubx_config_get_data_ptr(b, "lua_file", &lua_file_len);
lua_file = (!strncmp(lua_file, "", lua_file_len)) ? NULL : lua_file;
lua_str = (char *) ubx_config_get_data_ptr(b, "lua_str", &lua_str_len);
lua_str = (!strncmp(lua_str, "", lua_str_len)) ? NULL : lua_str;
if((inf->exec_str_buff = ubx_data_alloc(b->ni, "char", EXEC_STR_BUFF_SIZE)) == NULL) {
ERR("failed to allocate exec_str buffer");
goto out_free1;
}
if(init_lua_state(inf, lua_file, lua_str) != 0)
goto out_free2;
if((ret=call_hook(b, "init", 0, 1)) != 0)
goto out_free2;
/* Ok! */
ret = 0;
goto out;
out_free2:
free(inf->exec_str_buff);
out_free1:
free(inf);
out:
return ret;
}
/* init */
int zmq_sender_init(ubx_block_t *b)
{
int ret = -1;
struct zmq_sender_info *inf;
unsigned int tmplen;
char *connection_spec_str;
std::string connection_spec;
/* allocate memory for the block local state */
if ((inf = (struct zmq_sender_info*)calloc(1, sizeof(struct zmq_sender_info)))==NULL) {
ERR("zmq_sender: failed to alloc memory");
ret=EOUTOFMEM;
goto out;
}
b->private_data=inf;
update_port_cache(b, &inf->ports);
inf->buffer_length = DEFAULT_BUFFER_LENGTH; //TODO read from config
inf->buffer = new unsigned char [inf->buffer_length];
try {
inf->context = new zmq::context_t(1);
inf->publisher = new zmq::socket_t(*inf->context, ZMQ_PUB);
connection_spec_str = (char*) ubx_config_get_data_ptr(b, "connection_spec", &tmplen);
connection_spec = std::string(connection_spec_str);
std::cout << "ZMQ connection configuration for block " << b->name << " is " << connection_spec << std::endl;
inf->publisher->bind(connection_spec_str);
} catch (std::exception e) {
std::cout << e.what() << " : " << zmq_strerror (errno) << std::endl;
goto out;
}
ret=0;
out:
return ret;
}
/* init */
int zmq_reciever_init(ubx_block_t *b)
{
int ret = -1;
struct zmq_reciever_info *inf;
unsigned int tmplen;
char *connection_spec_str;
std::string connection_spec;
/* allocate memory for the block local state */
if ((inf = (struct zmq_reciever_info*)calloc(1, sizeof(struct zmq_reciever_info)))==NULL) {
ERR("zmq_reciever: failed to alloc memory");
ret=EOUTOFMEM;
goto out;
}
b->private_data=inf;
update_port_cache(b, &inf->ports);
try {
inf->context = new zmq::context_t(1);
inf->subscriber = new zmq::socket_t(*inf->context, ZMQ_SUB);
connection_spec_str = (char*) ubx_config_get_data_ptr(b, "connection_spec", &tmplen);
connection_spec = std::string(connection_spec_str);
std::cout << "ZMQ connection configuration for block " << b->name << " is " << connection_spec << std::endl;
inf->subscriber->connect(connection_spec_str);
inf->subscriber->setsockopt(ZMQ_SUBSCRIBE, "1", 0); // message filter options
} catch (std::exception e) {
std::cout << e.what() << " : " << zmq_strerror (errno) << std::endl;
goto out;
}
ret=0;
out:
return ret;
}
/* init */
int zmq_sender_init(ubx_block_t *b)
{
int ret = -1;
struct zmq_sender_info *inf;
unsigned int tmplen;
char *connection_spec_str;
std::string connection_spec;
// czmq socket for publisher
zsock_t* pub;
/* allocate memory for the block local state */
if ((inf = (struct zmq_sender_info*)calloc(1, sizeof(struct zmq_sender_info)))==NULL) {
ERR("zmq_sender: failed to alloc memory");
ret=EOUTOFMEM;
goto out;
}
// add the zmq_sender_info struct to private data to be able to access it in start, step and stop. alternatively (and better) this can be accomplished using an internal iblock (see https://github.com/ejans/internal_iblock_example)
b->private_data=inf;
update_port_cache(b, &inf->ports);
inf->buffer_length = DEFAULT_BUFFER_LENGTH; //TODO read from config
inf->buffer = new unsigned char [inf->buffer_length];
connection_spec_str = (char*) ubx_config_get_data_ptr(b, "connection_spec", &tmplen);
printf("ZMQ connection configuration for block %s is %s\n", b->name, connection_spec_str);
// creating the publisher based on the connection configuration, e.g. tcp://*:11411 or ipc://data
pub = zsock_new_pub(connection_spec_str);
if(!pub)
goto out;
// add a pointer to the publisher to the private data
inf->publisher=pub;
ret=0;
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;
}
static int youbot_init(ubx_block_t *b)
{
int ret=-1, i, cur_slave;
unsigned int len;
char* interface;
uint32_t *conf_nr_arms;
struct youbot_info *inf;
interface = (char *) ubx_config_get_data_ptr(b, "ethernet_if", &len);
conf_nr_arms = (uint32_t *) ubx_config_get_data_ptr(b, "nr_arms", &len);
if(strncmp(interface, "", len)==0) {
ERR("config ethernet_if unset");
goto out;
}
/* allocate driver data */
if((inf=calloc(1, sizeof(struct youbot_info)))==NULL) {
ERR("failed to alloc youbot_info");
goto out;
}
b->private_data=inf;
if(ec_init(interface) <= 0) {
ERR("ec_init failed (IF=%s), sufficient rights, correct interface?", interface);
goto out_free;
}
if(ec_config(TRUE, &inf->io_map) <= 0) {
ERR("ec_config failed (IF=%s)", interface);
goto out_free;
} else {
DBG("detected %d slaves", ec_slavecount);
}
/* Let's see what we have...
* check for base, base is mandatory for a youbot */
if(ec_slavecount >= YOUBOT_NR_OF_BASE_SLAVES && validate_base_slaves(1) == 0) {
DBG("detected youbot base");
inf->base.detected=1;
for(i=0, cur_slave=2; i<YOUBOT_NR_OF_WHEELS; i++, cur_slave++)
inf->base.wheel_inf[i].slave_idx=cur_slave;
} else {
ERR("no base detected");
goto out_free;
}
/* check for first arm */
if(ec_slavecount >= (YOUBOT_NR_OF_BASE_SLAVES + YOUBOT_NR_OF_ARM_SLAVES) &&
validate_arm_slaves(1+YOUBOT_NR_OF_BASE_SLAVES)==0) {
DBG("detected youbot arm #1 (first slave=%d)", cur_slave);
inf->arm1.detected=1;
*conf_nr_arms=1;
cur_slave++; /* skip the power board */
for(i=0; i<YOUBOT_NR_OF_JOINTS; i++, cur_slave++)
inf->arm1.jnt_inf[i].slave_idx=cur_slave;
}
/* check for second arm */
if(ec_slavecount >= (YOUBOT_NR_OF_BASE_SLAVES + (2 * YOUBOT_NR_OF_ARM_SLAVES)) &&
validate_arm_slaves(1+YOUBOT_NR_OF_BASE_SLAVES+YOUBOT_NR_OF_ARM_SLAVES)==0) {
DBG("detected youbot arm #2 (first slave=%d)", cur_slave);
inf->arm2.detected=1;
*conf_nr_arms=2;
cur_slave++; /* skip the power board */
for(i=0; i<YOUBOT_NR_OF_JOINTS; i++, cur_slave++)
inf->arm2.jnt_inf[i].slave_idx=cur_slave;
}
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
if(ec_slave[0].state != EC_STATE_SAFE_OP) {
ERR("not all slaves reached state safe_operational:");
for (i=0; i<=ec_slavecount; i++) {
if (ec_slave[i].state != EC_STATE_SAFE_OP) {
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_free;
}
/* send and receive bogus process data to get things going */
if(ec_send_processdata() <= 0) {
ERR("failed to send bootstrap process data");
goto out_free;
}
if(ec_receive_processdata(EC_TIMEOUTRET) == 0) {
ERR("failed to receive bootstrap process data");
goto out_free;
}
if(inf->base.detected) base_config_params(&inf->base);
if(inf->arm1.detected) arm_config_params(&inf->arm1);
if(inf->arm2.detected) arm_config_params(&inf->arm2);
#if 0
int tmp
/* find firmware version */
if(send_mbx(0, FIRMWARE_VERSION, 1, 0, 0, &tmp)) {
ERR("Failed to read firmware version");
goto out;
}
else
DBG("youbot firmware version %d", tmp);
#endif
ret=0;
goto out;
out_free:
free(b->private_data);
out:
return ret;
}
