bool AnalogServer::attachAll(const PolyDriverList &analog2attach)
{
yTrace();
if (analog2attach.size() != 1)
{
std::cerr<<"AnalogServer: cannot attach more than one device\n";
return false;
}
yarp::dev::PolyDriver * Idevice2attach=analog2attach[0]->poly;
if (Idevice2attach->isValid())
{
Idevice2attach->view(analogSensor_p);
}
if(NULL == analogSensor_p)
{
yError() << "AnalogServer: subdevice passed to attach method is invalid!!!";
return false;
}
attach(analogSensor_p);
start();
return true;
}
bool JoypadControlServer::attachAll(const PolyDriverList& p)
{
if (p.size() != 1)
{
yError("JoypadControlServer: cannot attach more than one device");
return false;
}
yarp::dev::PolyDriver* Idevice2attach = p[0]->poly;
if(p[0]->key == "IJoypadController")
{
yInfo() << "JoypadControlServer: Good name Dude!";
}
else
{
yInfo() << "JoypadControlServer: Bad name Dude!!";
}
if (!Idevice2attach->isValid())
{
yError() << "JoypadControlServer: Device " << p[0]->key << " to attach to is not valid ... cannot proceed";
return false;
}
Idevice2attach->view(m_device);
if(!attach(m_device))
return false;
PeriodicThread::setPeriod(m_period);
if (!PeriodicThread::start())
return false;
openPorts();
return true;
}
bool BatteryWrapper::attachAll(const PolyDriverList &battery2attach)
{
if (battery2attach.size() != 1)
{
yError("BatteryWrapper: cannot attach more than one device");
return false;
}
yarp::dev::PolyDriver * Idevice2attach = battery2attach[0]->poly;
if (Idevice2attach->isValid())
{
Idevice2attach->view(battery_p);
}
if(NULL == battery_p)
{
yError("BatteryWrapper: subdevice passed to attach method is invalid");
return false;
}
attach(battery_p);
RateThread::setRate(_rate);
RateThread::start();
return true;
}
bool AnalogWrapper::attachAll(const PolyDriverList &analog2attach)
{
//check if we already instantiated a subdevice previously
if (ownDevices)
return false;
if (analog2attach.size() != 1)
{
yError("AnalogWrapper: cannot attach more than one device");
return false;
}
yarp::dev::PolyDriver * Idevice2attach=analog2attach[0]->poly;
if (Idevice2attach->isValid())
{
Idevice2attach->view(analogSensor_p);
}
if(YARP_NULLPTR == analogSensor_p)
{
yError("AnalogWrapper: subdevice passed to attach method is invalid");
return false;
}
attach(analogSensor_p);
RateThread::setRate(_rate);
RateThread::start();
return true;
}
bool yarp::dev::ServerInertial::attachAll(const PolyDriverList &imuToAttachTo)
{
if (imuToAttachTo.size() != 1)
{
yError("ServerInertial: cannot attach more than one device");
return false;
}
return attach(imuToAttachTo[0]->poly);
}
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 Rangefinder2DWrapper::attachAll(const PolyDriverList &device2attach)
{
if (device2attach.size() != 1)
{
yError("Rangefinder2DWrapper: cannot attach more than one device");
return false;
}
yarp::dev::PolyDriver * Idevice2attach = device2attach[0]->poly;
if (Idevice2attach->isValid())
{
Idevice2attach->view(sens_p);
Idevice2attach->view(iTimed);
}
if (NULL == sens_p)
{
yError("Rangefinder2DWrapper: subdevice passed to attach method is invalid");
return false;
}
attach(sens_p);
if(!sens_p->getDistanceRange(minDistance, maxDistance))
{
yError() << "Laser device does not provide min & max distance range.";
return false;
}
if(!sens_p->getScanLimits(minAngle, maxAngle))
{
yError() << "Laser device does not provide min & max angle scan range.";
return false;
}
if (!sens_p->getHorizontalResolution(resolution))
{
yError() << "Laser device does not provide horizontal resolution ";
return false;
}
RateThread::setRate(_rate);
RateThread::start();
return true;
}
bool VirtualAnalogWrapper::attachAll(const PolyDriverList &polylist)
{
mMutex.wait();
for (int p=0; p<polylist.size(); ++p)
{
std::string key=polylist[p]->key.c_str();
// find appropriate entry in list of subdevices and attach
for (unsigned int k=0; k<mSubdevices.size(); ++k)
{
if (mSubdevices[k].getKey()==key)
{
if (!mSubdevices[k].attach(polylist[p]->poly,key))
{
mMutex.post();
return false;
}
}
}
}
//check if all devices are attached to the driver
for (unsigned int k=0; k<mSubdevices.size(); ++k)
{
if (!mSubdevices[k].isAttached())
{
mMutex.post();
return false;
}
}
mMutex.post();
Thread::start();
return true;
}
/**
* IWrapper and IMultipleWrapper interfaces
*/
bool RGBDSensorWrapper::attachAll(const PolyDriverList &device2attach)
{
// First implementation only accepts devices with both the interfaces Framegrabber and IDepthSensor,
// on a second version maybe two different devices could be accepted, one for each interface.
// Yet to be defined which and how parameters shall be used in this case ... using the name of the
// interface to view could be a good initial guess.
if (device2attach.size() != 1)
{
yError("RGBDSensorWrapper: cannot attach more than one device");
return false;
}
yarp::dev::PolyDriver * Idevice2attach = device2attach[0]->poly;
if(device2attach[0]->key == "IRGBDSensor")
{
yInfo() << "RGBDSensorWrapper: Good name Dude!";
}
else
{
yInfo() << "RGBDSensorWrapper: Bad name Dude!!";
}
if (!Idevice2attach->isValid())
{
yError() << "RGBDSensorWrapper: Device " << device2attach[0]->key << " to attach to is not valid ... cannot proceed";
return false;
}
Idevice2attach->view(sensor_p);
if(!attach(sensor_p))
return false;
RateThread::setRate(rate);
RateThread::start();
return true;
}
bool HapticDeviceWrapper::attachAll(const PolyDriverList &p)
{
if (p.size()!=1)
{
yError("*** Haptic Device Wrapper: cannot attach more than one device");
return false;
}
PolyDriver *dev=p[0]->poly;
if (dev->isValid())
dev->view(device);
if (device==NULL)
{
yError("*** Haptic Device Wrapper: invalid device");
return false;
}
start();
if (verbosity>0)
yInfo("*** Haptic Device Wrapper: started");
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;
}
