/* 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; }