bool floatingBaseEstimator::attachAllControlBoard(const PolyDriverList& p)
{
PolyDriverList controlBoardList;
for(size_t devIdx = 0; devIdx < (size_t) p.size(); devIdx++)
{
IEncoders * pEncs = 0;
if( p[devIdx]->poly->view(pEncs) )
{
controlBoardList.push(const_cast<PolyDriverDescriptor&>(*p[devIdx]));
}
}
// Attach the controlBoardList to the controlBoardRemapper
bool ok = remappedControlBoardInterfaces.multwrap->attachAll(controlBoardList);
if( !ok )
{
yError() << " floatingBaseEstimator::attachAll in attachAll of the remappedControlBoard";
return false;
}
return true;
}
bool RemoteControlBoardRemapper::open(Searchable& config)
{
Property prop;
prop.fromString(config.toString().c_str());
std::string localPortPrefix;
std::vector<std::string> remoteControlBoardsPorts;
// Check if the required parameters are found
if( prop.check("localPortPrefix") && prop.find("localPortPrefix").isString() )
{
localPortPrefix = prop.find("localPortPrefix").asString().c_str();
}
else
{
yError() <<"RemoteControlBoardRemapper: Error parsing parameters: \"localPortPrefix\" should be a string.";
return false;
}
Bottle *remoteControlBoards=prop.find("remoteControlBoards").asList();
if(remoteControlBoards==0)
{
yError() <<"RemoteControlBoardRemapper: Error parsing parameters: \"remoteControlBoards\" should be followed by a list.";
return false;
}
remoteControlBoardsPorts.resize(remoteControlBoards->size());
for(int ax=0; ax < remoteControlBoards->size(); ax++)
{
remoteControlBoardsPorts[ax] = remoteControlBoards->get(ax).asString().c_str();
}
// Load the REMOTE_CONTROLBOARD_OPTIONS, containg any additional option to pass to the remote control boards
Property remoteControlBoardsOptions;
Bottle & optionsGroupBot = prop.findGroup("REMOTE_CONTROLBOARD_OPTIONS");
if( !(optionsGroupBot.isNull()) )
{
remoteControlBoardsOptions.fromString(optionsGroupBot.toString());
}
// Parameters loaded, open all the remote controlboards
m_remoteControlBoardDevices.resize(remoteControlBoardsPorts.size(),0);
PolyDriverList remoteControlBoardsList;
for(size_t ctrlBrd=0; ctrlBrd < remoteControlBoardsPorts.size(); ctrlBrd++ )
{
std::string remote = remoteControlBoardsPorts[ctrlBrd];
// Note: as local parameter we use localPortPrefix+remoteOfTheReportControlBoard
std::string local = localPortPrefix+remote;
Property options = remoteControlBoardsOptions;
options.put("device", "remote_controlboard");
options.put("local", local);
options.put("remote", remote);
m_remoteControlBoardDevices[ctrlBrd] = new PolyDriver();
bool ok = m_remoteControlBoardDevices[ctrlBrd]->open(options);
if( !ok || !(m_remoteControlBoardDevices[ctrlBrd]->isValid()) )
{
yError() << "RemoteControlBoardRemapper: error opening remote_controlboard with remote \"" << remote << "\", opening the device failed.";
closeAllRemoteControlBoards();
return false;
}
// We use the remote name of the remote_controlboard as the key for it, in absense of anything better
remoteControlBoardsList.push((m_remoteControlBoardDevices[ctrlBrd]),remote.c_str());
}
// Device opened, now we open the ControlBoardRemapper and then we call attachAll
bool ok = ControlBoardRemapper::open(prop);
if( !ok )
{
yError() << "RemoteControlBoardRemapper: error opening the controlboardremapper device, opening the device failed.";
ControlBoardRemapper::close();
closeAllRemoteControlBoards();
return false;
}
// If open went ok, we now call attachAll
ok = ControlBoardRemapper::attachAll(remoteControlBoardsList);
if( !ok )
{
yError() << "RemoteControlBoardRemapper: error calling attachAll in the controlboardremapper device, opening the device failed.";
ControlBoardRemapper::close();
closeAllRemoteControlBoards();
return false;
}
// All went ok, return true
// TODO: close devices that are not actually used by the remapper
return true;
}
bool Rangefinder2DWrapper::open(yarp::os::Searchable &config)
{
Property params;
params.fromString(config.toString().c_str());
if (!config.check("period"))
{
yError() << "Rangefinder2DWrapper: missing 'period' parameter. Check you configuration file\n";
return false;
}
else
_rate = config.find("period").asInt();
if (!config.check("name"))
{
yError() << "Rangefinder2DWrapper: missing 'name' parameter. Check you configuration file; it must be like:";
yError() << " name: full name of the port, like /robotName/deviceId/sensorType:o";
return false;
}
else
{
streamingPortName = config.find("name").asString().c_str();
rpcPortName = streamingPortName + "/rpc:i";
setId("Rangefinder2DWrapper");
}
if(!initialize_YARP(config) )
{
yError() << sensorId << "Error initializing YARP ports";
return false;
}
checkROSParams(config);
// call ROS node/topic initilization, if needed
if (!initialize_ROS())
{
return false;
}
if(config.check("subdevice"))
{
Property p;
PolyDriverList driverlist;
p.fromString(config.toString(), false);
p.put("device", config.find("subdevice").asString());
if(!driver.open(p) || !driver.isValid())
{
yError() << "RangeFinder2DWrapper: failed to open subdevice.. check params";
return false;
}
driverlist.push(&driver, "1");
if(!attachAll(driverlist))
{
yError() << "RangeFinder2DWrapper: failed to open subdevice.. check params";
return false;
}
isDeviceOwned = true;
}
return true;
}
bool TwoCanBusThreeWrappers::configure(ResourceFinder &rf) {
if(rf.check("help")) {
printf("TwoCanBusThreeWrappers options:\n");
printf("\t--help (this help)\t--from [file.ini]\t--context [path]\n");
CD_DEBUG_NO_HEADER("%s\n",rf.toString().c_str());
return false;
}
//-- /dev/can0 --
Bottle devCan0 = rf.findGroup("devCan0");
CD_DEBUG("%s\n",devCan0.toString().c_str());
Property optionsDevCan0;
optionsDevCan0.fromString(devCan0.toString());
deviceDevCan0.open(optionsDevCan0);
if (!deviceDevCan0.isValid()) {
CD_ERROR("deviceDevCan0 instantiation not worked.\n");
return false;
}
//-- /dev/can1 --
Bottle devCan1 = rf.findGroup("devCan1");
CD_DEBUG("%s\n",devCan1.toString().c_str());
Property optionsDevCan1;
optionsDevCan1.fromString(devCan1.toString());
deviceDevCan1.open(optionsDevCan1);
if (!deviceDevCan1.isValid()) {
CD_ERROR("deviceDevCan1 instantiation not worked.\n");
return false;
}
//-- wrapper0 --
Bottle wrapper0 = rf.findGroup("wrapper0");
CD_DEBUG("%s\n",wrapper0.toString().c_str());
Property optionsWrapper0;
optionsWrapper0.fromString(wrapper0.toString());
deviceWrapper0.open(optionsWrapper0);
if (!deviceWrapper0.isValid()) {
CD_ERROR("deviceWrapper0 instantiation not worked.\n");
return false;
}
//-- wrapper1 --
Bottle wrapper1 = rf.findGroup("wrapper1");
CD_DEBUG("%s\n",wrapper1.toString().c_str());
Property optionsWrapper1;
optionsWrapper1.fromString(wrapper1.toString());
deviceWrapper1.open(optionsWrapper1);
if (!deviceWrapper1.isValid()) {
CD_ERROR("deviceWrapper1 instantiation not worked.\n");
return false;
}
//-- wrapper2 --
Bottle wrapper2 = rf.findGroup("wrapper2");
CD_DEBUG("%s\n",wrapper2.toString().c_str());
Property optionsWrapper2;
optionsWrapper2.fromString(wrapper2.toString());
deviceWrapper2.open(optionsWrapper2);
if (!deviceWrapper2.isValid()) {
CD_ERROR("deviceWrapper2 instantiation not worked.\n");
return false;
}
IMultipleWrapper *iWrapper0, *iWrapper1, *iWrapper2;
deviceWrapper0.view(iWrapper0);
deviceWrapper1.view(iWrapper1);
deviceWrapper2.view(iWrapper2);
PolyDriverList list;
list.push(&deviceDevCan0, "devCan0");
list.push(&deviceDevCan1, "devCan1");
iWrapper0->attachAll(list);
iWrapper1->attachAll(list);
iWrapper2->attachAll(list);
return true;
}
