/* 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;
}
/**
* luablock_step - execute lua string and call step hook
*
* @param b
*/
static void luablock_step(ubx_block_t *b)
{
int len=0, ret;
struct luablock_info* inf = (struct luablock_info*) b->private_data;
/* any lua code to execute */
ubx_port_t* p_exec_str = ubx_port_get(b, "exec_str");
if((len=__port_read(p_exec_str, inf->exec_str_buff))>0) {
if ((ret=luaL_dostring(inf->L, inf->exec_str_buff->data)) != 0) {
ERR("Failed to exec_str: %s", lua_tostring(inf->L, -1));
goto out;
}
}
call_hook(b, "step", 0, 0);
out:
/* TODO: fix this. realloc could have changed port addr */
if(len>0) {
p_exec_str = ubx_port_get(b, "exec_str");
write_int(p_exec_str, &ret);
}
return;
}
/* step */
void random_step(ubx_block_t *b)
{
unsigned int rand_val;
struct rnd_info* inf;
inf=(struct rnd_info*) b->private_data;
ubx_port_t* rand_port = ubx_port_get(b, "rnd");
rand_val = random();
rand_val = (rand_val > inf->info.max) ? (rand_val%inf->info.max) : rand_val;
rand_val = (rand_val < inf->info.min) ? ((inf->info.min + rand_val)%inf->info.max) : rand_val;
write_rnd(rand_port, &rand_val);
}
static int youbot_start(ubx_block_t *b)
{
DBG(" ");
int ret=-1, i;
struct youbot_info *inf=b->private_data;
/* reset old commands */
if(inf->base.detected && base_prepare_start(&inf->base) != 0) goto out;
if(inf->arm1.detected && arm_prepare_start(&inf->arm1) != 0) goto out;
if(inf->arm2.detected && arm_prepare_start(&inf->arm2) != 0) goto out;
/* requesting operational for all slaves */
ec_slave[0].state = EC_STATE_OPERATIONAL;
ec_writestate(0);
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
if(ec_slave[0].state != EC_STATE_OPERATIONAL) {
ERR("not all slaves reached state operational:");
for (i=0; i<=ec_slavecount; i++) {
if (ec_slave[i].state != EC_STATE_OPERATIONAL) {
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;
}
/* get things rolling */
if (ec_send_processdata() <= 0) {
inf->pd_send_err++;
ERR("sending initial process data failed");
}
/* cache port pointers */
assert(inf->base.p_control_mode = ubx_port_get(b, "base_control_mode"));
assert(inf->base.p_cmd_twist = ubx_port_get(b, "base_cmd_twist"));
assert(inf->base.p_cmd_vel = ubx_port_get(b, "base_cmd_vel"));
assert(inf->base.p_cmd_cur = ubx_port_get(b, "base_cmd_cur"));
assert(inf->base.p_msr_odom = ubx_port_get(b, "base_msr_odom"));
assert(inf->base.p_msr_twist = ubx_port_get(b, "base_msr_twist"));
assert(inf->base.p_base_motorinfo = ubx_port_get(b, "base_motorinfo"));
assert(inf->arm1.p_control_mode = ubx_port_get(b, "arm1_control_mode"));
assert(inf->arm1.p_calibrate_cmd = ubx_port_get(b, "arm1_calibrate_cmd"));
assert(inf->arm1.p_cmd_pos = ubx_port_get(b, "arm1_cmd_pos"));
assert(inf->arm1.p_cmd_vel = ubx_port_get(b, "arm1_cmd_vel"));
assert(inf->arm1.p_cmd_cur = ubx_port_get(b, "arm1_cmd_cur"));
assert(inf->arm1.p_cmd_eff = ubx_port_get(b, "arm1_cmd_eff"));
assert(inf->arm1.p_arm_state = ubx_port_get(b, "arm1_state"));
assert(inf->arm1.p_arm_motorinfo = ubx_port_get(b, "arm1_motorinfo"));
assert(inf->arm1.p_gripper = ubx_port_get(b, "arm1_gripper"));
ret=0;
out:
return ret;
}
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;
}
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;
}
/* declare a helper function to update the port cache this is necessary
* because the port ptrs can change if ports are dynamically added or
* removed. This function should hence be called after all
* initialization is done, i.e. typically in 'start'
*/
static void update_port_cache(ubx_block_t *b, struct zmq_server_port_cache *pc)
{
pc->zmq_req = ubx_port_get(b, "zmq_req");
pc->zmq_rep = ubx_port_get(b,"zmq_rep");
}
/* declare a helper function to update the port cache this is necessary
* because the port ptrs can change if ports are dynamically added or
* removed. This function should hence be called after all
* initialization is done, i.e. typically in 'start'
*/
static void update_port_cache(ubx_block_t *b, struct rsg_dump_port_cache *pc)
{
pc->rsg_out = ubx_port_get(b, "rsg_out");
}
/* declare a helper function to update the port cache this is necessary
* because the port ptrs can change if ports are dynamically added or
* removed. This function should hence be called after all
* initialization is done, i.e. typically in 'start'
*/
static void update_port_cache(ubx_block_t *b, struct rsg_json_reciever_port_cache *pc)
{
pc->rsg_in = ubx_port_get(b, "rsg_in");
}