Ejemplo n.º 1
0
/* 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;
}
Ejemplo n.º 2
0
/**
 * 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;
}
Ejemplo n.º 3
0
/* 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);
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 5
0
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;
}
Ejemplo n.º 6
0
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;
}
Ejemplo n.º 7
0
/* 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");
}
Ejemplo n.º 8
0
/* 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");
}