bool ServerSerial::open(Searchable& prop)
{
verb = (prop.check("verbose",Value(0),"Specifies if the device is in verbose mode (0/1).").asInt())>0;
if (verb)
printf("running with verbose output\n");
Value *name;
if (prop.check("subdevice",name,"name of specific control device to wrap")) {
printf("Subdevice %s\n", name->toString().c_str());
if (name->isString()) {
// maybe user isn't doing nested configuration
Property p;
p.setMonitor(prop.getMonitor(),
"subdevice"); // pass on any monitoring
p.fromString(prop.toString());
p.put("device",name->toString());
poly.open(p);
} else {
Bottle subdevice = prop.findGroup("subdevice").tail();
poly.open(subdevice);
}
if (!poly.isValid()) {
printf("cannot make <%s>\n", name->toString().c_str());
}
} else {
printf("\"--subdevice <name>\" not set for server_serial\n");
return false;
}
if (!poly.isValid()) {
return false;
}
ConstString rootName =
prop.check("name",Value("/serial"),
"prefix for port names").asString().c_str();
command_buffer.attach(toDevice);
reply_buffer.attach(fromDevice);
command_buffer.useCallback(callback_impl);
toDevice.open((rootName+"/in").c_str());
fromDevice.open((rootName+"/out").c_str());
if (poly.isValid())
poly.view(serial);
if(serial != NULL) {
start();
return true;
}
printf("subdevice <%s> doesn't look like a serial port (no appropriate interfaces were acquired)\n",
name->toString().c_str());
return false;
}
bool JoypadControlServer::openAndAttachSubDevice(Searchable& prop)
{
Property p;
m_subDeviceOwned = new PolyDriver;
p.fromString(prop.toString().c_str());
p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.unput("device");
p.put("device",prop.find("subdevice").asString()); // subdevice was already checked before
// if error occour during open, quit here.
m_subDeviceOwned->open(p);
if (!m_subDeviceOwned->isValid())
{
yError("JoypadControlServer: opening subdevice... FAILED\n");
return false;
}
m_isSubdeviceOwned = true;
if(!attach(m_subDeviceOwned))
return false;
if(!m_parser.configure(m_device) )
{
yError() << "JoypadControlServer: error configuring interfaces for parsers";
return false;
}
openPorts();
PeriodicThread::setPeriod(m_period);
return PeriodicThread::start();
}
bool RGBDSensorWrapper::openAndAttachSubDevice(Searchable& prop)
{
Property p;
subDeviceOwned = new PolyDriver;
p.fromString(prop.toString().c_str());
p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.unput("device");
p.put("device",prop.find("subdevice").asString()); // subdevice was already checked before
// if error occour during open, quit here.
yDebug("opening IRGBDSensor subdevice\n");
subDeviceOwned->open(p);
if (!subDeviceOwned->isValid())
{
yError("opening controlBoardWrapper2 subdevice... FAILED\n");
return false;
}
isSubdeviceOwned = true;
if(!attach(subDeviceOwned))
return false;
RateThread::setRate(rate);
RateThread::start();
return true;
}
bool HapticDeviceWrapper::open(Searchable &config)
{
portStemName=config.check("name",
Value(HAPTICDEVICE_WRAPPER_DEFAULT_NAME)).asString().c_str();
verbosity=config.check("verbosity",Value(0)).asInt();
int period=config.check("period",
Value(HAPTICDEVICE_WRAPPER_DEFAULT_PERIOD)).asInt();
setRate(period);
if (config.check("subdevice"))
{
Property p(config.toString().c_str());
p.setMonitor(config.getMonitor(),"subdevice");
p.unput("device");
p.put("device",config.find("subdevice").asString());
if (driver.open(p))
{
IHapticDevice *d;
driver.view(d);
attach(d);
}
else
{
yError("*** Haptic Device Wrapper: failed to open the driver!");
return false;
}
}
if (verbosity>0)
yInfo("*** Haptic Device Wrapper: opened");
return true;
}
ResourceFinder::ResourceFinder(Searchable& data, void *implementation) {
this->implementation = implementation;
if (!data.isNull()) {
config.fromString(data.toString());
}
nullConfig = data.isNull();
owned = false;
isConfiguredFlag = true;
}
bool RGBDSensorWrapper::openAndAttachSubDevice(Searchable& prop)
{
Property p;
subDeviceOwned = new PolyDriver;
p.fromString(prop.toString().c_str());
p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.unput("device");
p.put("device",prop.find("subdevice").asString()); // subdevice was already checked before
// if error occour during open, quit here.
yDebug("opening IRGBDSensor subdevice\n");
subDeviceOwned->open(p);
if (!subDeviceOwned->isValid())
{
yError("opening controlBoardWrapper2 subdevice... FAILED\n");
return false;
}
isSubdeviceOwned = true;
if(!attach(subDeviceOwned))
return false;
// Configuring parsers
IRgbVisualParams * rgbVis_p;
IDepthVisualParams * depthVis_p;
subDeviceOwned->view(rgbVis_p);
subDeviceOwned->view(depthVis_p);
if(!parser.configure(sensor_p) )
{
yError() << "RGBD wrapper: error configuring interfaces for parsers";
return false;
}
/*
bool conf = rgbParser.configure(rgbVis_p);
conf &= depthParser.configure(depthVis_p);
if(!conf)
{
yError() << "RGBD wrapper: error configuring interfaces for parsers";
return false;
}
*/
RateThread::setRate(rate);
RateThread::start();
return true;
}
bool ReachManager::open(Searchable& config)
{
cout << "config : " << config.toString() << endl;
parameters["input_port"] = new Value(GetValueFromConfig(config, "input_port"));
parameters["output_port"] = new Value(GetValueFromConfig(config, "output_port"));
parameters["robot"] = new Value(GetValueFromConfig(config, "robot"));
parameters["num_dof"] = new Value(GetValueFromConfig(config, "num_dof"));
parameters["reach_command_code"] = new Value(GetValueFromConfig(config, "reach_command_code"));
parameters["max_error"] = new Value(GetValueFromConfig(config, "max_error"));
parameters["solver_name"] = new Value(GetValueFromConfig(config, "solver_name"));
parameters["enabled_arm"] = new Value(GetValueFromConfig(config, "enabled_arm"));
parameters["pos_vel_cont"] = new Value(GetValueFromConfig(config, "pos_vel_cont"));
parameters["min_reach_dist"] = new Value(GetValueFromConfig(config, "min_reach_dist"));
cout << "min reach dist : " << parameters["min_reach_dist"]->asDouble() << endl;
parameters["reach_mode_dist"] = new Value(GetValueFromConfig(config, "reach_mode_dist"));
cout << "reach mode dist : " << parameters["reach_mode_dist"]->asDouble() << endl;
parameters["object_ID"] = new Value(GetValueFromConfig(config, "object_ID"));
//cout << "cool !" << endl;
inPort.open(parameters["input_port"]->asString().c_str());
outPort.open(parameters["output_port"]->asString().c_str());
//Network::connect(outPort.getName().c_str(), "/manager");
outFile.open("reaching.dat");
if(parameters["enabled_arm"]->asString() == "left")
{
OpenIKSolver("left");
}
else if(parameters["enabled_arm"]->asString() == "right")
{
OpenIKSolver("right");
}
else if(parameters["enabled_arm"]->asString() == "both")
{
OpenIKSolver("left");
OpenIKSolver("right");
}
if(parameters["pos_vel_cont"]->asInt())
{
InitPositionControl("right");
InitPositionControl("left");
}
return true;
}
bool AnalogWrapper::openAndAttachSubDevice(Searchable &prop)
{
Property p;
subDeviceOwned = new PolyDriver;
p.fromString(prop.toString().c_str());
// p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring
p.unput("device");
p.put("device", prop.find("subdevice").asString()); // subdevice was already checked before
// if error occour during open, quit here.
yDebug("opening analogServer subdevice...");
subDeviceOwned->open(p);
if (!subDeviceOwned->isValid())
{
yError("opening analogServer subdevice... FAILED\n");
return false;
}
subDeviceOwned->view(analogSensor_p);
if (analogSensor_p == 0)
{
yError("Opening IAnalogSensor interface of analogServer subdevice... FAILED\n");
return false;
}
int chNum = analogSensor_p->getChannels();
if (chNum <= 0)
{
yError("Calling analog sensor has invalid channels number %d.\n", chNum);
return false;
}
attach(analogSensor_p);
RateThread::setRate(_rate);
RateThread::start();
return true;
}
bool AcousticMap::open(Searchable& config) {
bool ok = true;
if(!config.check("name")) {
std::cout << "AuditoryMap: Error, module base name not found in configuration. Start the module with the --name option.." << std::endl;
return false;
}
// module base name
std::string strModuleName = std::string(config.find("name").asString().c_str());
// look for group EGO_SPHERE_ACOUSTIC_MAP
Bottle botConfigAcoustic(config.toString().c_str());
botConfigAcoustic.setMonitor(config.getMonitor());
if (!config.findGroup("EGO_SPHERE_ACOUSTIC_MAP").isNull()) {
botConfigAcoustic.clear();
botConfigAcoustic.fromString(config.findGroup("EGO_SPHERE_ACOUSTIC_MAP", "Loading visual map configuration from group EGO_SPHERE_ACOUSTIC_MAP.").toString());
}
_salienceDecayRate = botConfigAcoustic.check("decayAcoustic",
Value(0.95),
"Decay for the acoustic saliency map (double).").asDouble();
_resXAcoustic = botConfigAcoustic.check("resXAcoustic",
Value(80),
"Width of internal acoustic map (int)").asInt();
_resYAcoustic = botConfigAcoustic.check("resYAcoustic",
Value(60),
"Height of internal acoustic map (int)").asInt();
_imgCart.resize(_resXAcoustic,_resYAcoustic);
_imgRemapX.resize(_resXAcoustic,_resYAcoustic);
_imgRemapY.resize(_resXAcoustic,_resYAcoustic);
_imgSpher.resize(_resXAcoustic,_resYAcoustic);
_imgMapResA.resize(_resXAcoustic,_resYAcoustic);
ok = ok && _prtVctSound.open(std::string(strModuleName + std::string("/mapAuditory/vct_in")).c_str());
return ok;
}
virtual bool open(Searchable &s)
{
Time::turboBoost();
// get command line options
options.fromString(s.toString());
if (!options.check("robot") || !options.check("part"))
{
printf("Missing either --robot or --part options. Quitting!\n");
return false;
}
std::string dumpername;
// get dumepr name
if (options.check("name"))
{
dumpername = options.find("name").asString();
dumpername += "/";
}
else
{
dumpername = "/controlBoardDumper/";
}
Value& robot = options.find("robot");
Value& part = options.find("part");
configFileRobotPart = "config_";
configFileRobotPart = configFileRobotPart + robot.asString();
configFileRobotPart = configFileRobotPart + "_";
configFileRobotPart = configFileRobotPart + part.asString();
configFileRobotPart = configFileRobotPart + ".ini";
//get the joints to be dumped
getRate(options, rate);
if (getNumberUsedJoints(options, nJoints) == 0)
return false;
thetaMap = new int[nJoints];
if (getUsedJointsMap(options, nJoints, thetaMap) == 0)
return false;
//get the type of data to be dumped
if (getNumberDataToDump(options, nData) == 0)
return false;
dataToDump = new std::string[nData];
if (getDataToDump(options, dataToDump, nData, &useDebugClient) == 0)
return false;
// Printing configuration to the user
yInfo("Running with the following configuration:\n");
yInfo("Selected rate is: %d\n", rate);
yInfo("Data selected to be dumped are:\n");
for (int i = 0; i < nData; i++)
yInfo("\t%s \n", dataToDump[i].c_str());
//open remote control board
ddBoardOptions.put("device", "remote_controlboard");
ddDebugOptions.put("device", "debugInterfaceClient");
ConstString localPortName = name;
ConstString localDebugPortName = name;
localPortName = localPortName + dumpername.c_str();
localDebugPortName = localPortName + "debug/";
//localPortName = localPortName + robot.asString();
localPortName = localPortName + part.asString();
localDebugPortName = localDebugPortName + part.asString();
ddBoardOptions.put("local", localPortName.c_str());
ddDebugOptions.put("local", localDebugPortName.c_str());
ConstString remotePortName = "/";
ConstString remoteDebugPortName;
remotePortName = remotePortName + robot.asString();
remotePortName = remotePortName + "/";
remotePortName = remotePortName + part.asString();
ddBoardOptions.put("remote", remotePortName.c_str());
remoteDebugPortName = remotePortName + "/debug";
ddDebugOptions.put("remote", remoteDebugPortName.c_str());
// create a device
ddBoard.open(ddBoardOptions);
if (!ddBoard.isValid()) {
printf("Device not available.\n");
Network::fini();
return false;
}
if (useDebugClient )
{
ddDebug.open(ddDebugOptions);
if (!ddDebug.isValid())
{
yError("\n-----------------------------------------\n");
yError("Debug Interface is mandatory to run this module with the '--dataToDumpAll' option or to dump any of the getRotorxxx data.\n");
yError("Please Verify the following 2 conditions are satisfied:\n\n");
yError("1) Check 'debugInterfaceClient' is available using 'yarpdev --list' command\n");
// yError("%s", Drivers::factory().toString().c_str());
std::string deviceList, myDev;
deviceList.clear();
deviceList.append(Drivers::factory().toString().c_str());
myDev = "debugInterfaceClient";
if(deviceList.find(myDev) != std::string::npos)
yError("\t--> Seems OK\n");
else
yError("\t--> Seems NOT OK. The device was not found, please activate the compilation using the corrisponding CMake flag.\n");
yError("\n2) Check if the robot has the 'debugInterfaceWrapper' device up and running. \n You should see from 'yarp name list' output, a port called\n");
yError("\t/robotName/part_name/debug/rpc:i\n If not, fix the robot configuration files to instantiate the 'debugInterfaceWrapper' device.\n");
yError("\nQuickFix: If you set the --dataToDumpAll and do not need the advanced debug feature (getRotorxxx) just remove this option. See help for more information.\n");
yError("------------- END ERROR MESSAGE ---------------\n\n");
Network::fini();
return false;
}
}
bool logToFile = false;
if (options.check("logToFile")) logToFile = true;
portPrefix= dumpername.c_str() + part.asString() + "/";
//boardDumperThread *myDumper = new boardDumperThread(&dd, rate, portPrefix, dataToDump[0]);
//myDumper->setThetaMap(thetaMap, nJoints);
myDumper = new boardDumperThread[nData];
for (int i = 0; i < nData; i++)
{
if (dataToDump[i] == "getEncoders" )
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getEncs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetEncs.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetEncs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetEncs);
}
}
else if (dataToDump[i] == "getEncoderSpeeds")
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getSpeeds thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetSpeeds.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
myGetSpeeds.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetSpeeds);
}
}
else if (dataToDump[i] == "getEncoderAccelerations")
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getAccs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetAccs.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetAccs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetAccs);
}
}
else if (dataToDump[i] == "getPosPidReferences")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPidRefs.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getPosPidReferences::The time stamp initalization interfaces was successfull! \n");
myGetPidRefs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPidRefs);
}
}
else if (dataToDump[i] == "getTrqPidReferences")
{
if (ddBoard.view(itrq))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqRefs.setInterface(itrq);
if (ddBoard.view(istmp))
{
yInfo("getTrqPidReferences::The time stamp initalization interfaces was successfull! \n");
myGetTrqRefs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqRefs);
}
}
else if (dataToDump[i] == "getControlModes")
{
if (ddBoard.view(icmod))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetControlModes.setInterface(icmod, nJoints);
if (ddBoard.view(istmp))
{
yInfo("getControlModes::The time stamp initalization interfaces was successfull! \n");
myGetControlModes.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetControlModes);
}
}
else if (dataToDump[i] == "getInteractionModes")
{
if (ddBoard.view(iimod))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetInteractionModes.setInterface(iimod, nJoints);
if (ddBoard.view(istmp))
{
yInfo("getInteractionModes::The time stamp initalization interfaces was successfull! \n");
myGetInteractionModes.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetInteractionModes);
}
}
else if (dataToDump[i] == "getPositionErrors")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPosErrs.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getPositionErrors::The time stamp initalization interfaces was successfull! \n");
myGetPosErrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPosErrs);
}
}
else if (dataToDump[i] == "getOutputs")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getOuts thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetOuts.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getOutputs::The time stamp initalization interfaces was successfull! \n");
myGetOuts.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetOuts);
}
}
else if (dataToDump[i] == "getCurrents")
{
if (ddBoard.view(iamp))
{
yInfo("Initializing a getCurrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetCurrs.setInterface(iamp);
if (ddBoard.view(istmp))
{
yInfo("getCurrents::The time stamp initalization interfaces was successfull! \n");
myGetCurrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetCurrs);
}
}
else if (dataToDump[i] == "getTorques")
{
if (ddBoard.view(itrq))
{
yInfo("Initializing a getTorques thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqs.setInterface(itrq);
if (ddBoard.view(istmp))
{
yInfo("getTorques::The time stamp initalization interfaces was successfull! \n");
myGetTrqs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqs);
}
}
else if (dataToDump[i] == "getTorqueErrors")
{
if (ddBoard.view(itrq))
{
yInfo("Initializing a getTorqueErrors thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqErrs.setInterface(itrq);
if (ddBoard.view(istmp))
{
yInfo("getTorqueErrors::The time stamp initalization interfaces was successfull! \n");
myGetTrqErrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqErrs);
}
}
else if (dataToDump[i] == "getRotorPositions")
{
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
yInfo("Initializing a getRotorPosition thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorPoss.setInterface(idbg);
if (ddDebug.view(istmp))
{
yInfo("getRotorPositions::The time stamp initalization interfaces was successfull! \n");
myGetRotorPoss.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorPoss);
}
else
{
yError("Debug Interface not available, cannot dump %s.\n", dataToDump[i].c_str());
Network::fini();
return false;
}
}
}
else if (dataToDump[i] == "getRotorSpeeds")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
yInfo("Initializing a getRotorSpeed thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorVels.setInterface(idbg);
if (ddDebug.view(istmp))
{
yInfo("getRotorSpeeds::The time stamp initalization interfaces was successfull! \n");
myGetRotorVels.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorVels);
}
else
{
printf("Debug Interface not available, cannot dump %s.\n", dataToDump[i].c_str());
Network::fini();
return false;
}
}
else if (dataToDump[i] == "getRotorAccelerations")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
yInfo("Initializing a getRotorAcceleration thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorAccs.setInterface(idbg);
if (ddDebug.view(istmp))
{
yInfo("getRotorAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetRotorAccs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorAccs);
}
else
{
printf("Debug Interface not available, cannot dump %s.\n", dataToDump[i].c_str());
Network::fini();
return false;
}
}
else if (dataToDump[i] == "getTemperatures")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getTemps thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTemps.setInterface(imot);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetTemps.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTemps);
}
}
else if (dataToDump[i] == "getMotorEncoders")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getEncs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorEncs.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetMotorEncs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorEncs);
}
}
else if (dataToDump[i] == "getMotorEncoderSpeeds")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getSpeeds thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorSpeeds.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
myGetMotorSpeeds.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorSpeeds);
}
}
else if (dataToDump[i] == "getMotorEncoderAccelerations")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getAccs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorAccs.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetMotorAccs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorAccs);
}
}
}
Time::delay(1);
for (int i = 0; i < nData; i++)
myDumper[i].start();
return true;
}
QWidgetModuleDefault::QWidgetModuleDefault(Searchable& config, QWidget* parent, const char* name, bool modal, WFlags fl)
: QWidget(parent, name, fl),
_qwViewer(NULL),
_qwOutput(NULL),
_qwRPC(NULL),
_qwConnections(NULL){
Property prop;
prop.fromString(config.toString());
if(!prop.check("name")){
prop.put("name", "/moduleGui");
}
this->setCaption("Yarp Default Module Interface");
// main layout (vbox)
QVBoxLayout *mainLayout = new QVBoxLayout( this, 3, 3, "QWidgetModuleDefaultBaseLayout");
// add a vertical split as first entry to main layout
QSplitter *splitMain = new QSplitter(this);
mainLayout->addWidget(splitMain);
// add horizontal splitter to left part of main splitter
QSplitter *splitLeft = new QSplitter(Qt::Vertical, splitMain);
Property propViewer; propViewer.fromString(prop.toString());
propViewer.put("name", std::string(std::string(propViewer.find("name").asString().c_str()) + std::string("/viewer")).c_str());
_qwViewer = new QWidgetViewer(propViewer, splitLeft); // the viewer widget
_qwConnections = new QWidgetConnections(prop, splitLeft); // the connection widget
// add horizontal splitter to right part of main splitter
QSplitter *splitRight = new QSplitter(Qt::Vertical, splitMain);
Property propOutput; propOutput.fromString(prop.toString());
propOutput.put("name", std::string(std::string(propOutput.find("name").asString().c_str()) + std::string("/stdout")).c_str());
_qwOutput = new QWidgetOutput(propOutput, splitRight); // the output widget
Property propRPC; propRPC.fromString(prop.toString());
propRPC.put("name", std::string(std::string(propRPC.find("name").asString().c_str()) + std::string("/rpc")).c_str());
_qwRPC = new QWidgetRPC(propRPC, splitRight); // the RPC widget
// add a frame as second entry in main layout
QFrame *frmBottom = new QFrame(this);
frmBottom->setMaximumHeight(35);
mainLayout->addWidget(frmBottom);
// add a layout to the bottom frame
QHBoxLayout *frmBottomLayout = new QHBoxLayout(frmBottom, 0, -1, "frmBottomLayout");
frmBottomLayout->setSpacing(3);
frmBottomLayout->setMargin(3);
// add a button to the bottom frame
QPushButton *btnCheckAll = new QPushButton(frmBottom);
btnCheckAll->setText("check all ports and connections");
frmBottomLayout->addWidget(btnCheckAll);
connect( btnCheckAll, SIGNAL( clicked() ), this, SLOT( btnCheckAll_clicked() ) );
this->resize(850,650);
QValueList<int> valsSplitMain;
valsSplitMain.append(400);
valsSplitMain.append(450);
splitMain->setSizes(valsSplitMain);
QValueList<int> valsSplitLeft;
valsSplitLeft.append(400);
valsSplitLeft.append(250);
splitLeft->setSizes(valsSplitLeft);
QValueList<int> valsSplitRight;
valsSplitRight.append(400);
valsSplitRight.append(250);
splitRight->setSizes(valsSplitRight);
// position screen center
QWidget* desk = QApplication::desktop();
this->move(desk->width()/2 - this->width()/2,desk->height()/2 - this->height()/2);
}
virtual bool open(Searchable &s)
{
Time::turboBoost();
// get command line options
options.fromString(s.toString());
if (!options.check("robot") || !options.check("part"))
{
printf("Missing either --robot or --part options. Quitting!\n");
return false;
}
// get command file options
Value& robot = options.find("robot");
Value& part = options.find("part");
configFileRobotPart = "config_";
configFileRobotPart = configFileRobotPart + robot.asString();
configFileRobotPart = configFileRobotPart + "_";
configFileRobotPart = configFileRobotPart + part.asString();
configFileRobotPart = configFileRobotPart + ".ini";
//get the joints to be dumped
getRate(options, rate);
fprintf(stderr, "Selected rate is: %d\n", rate);
if (getNumberUsedJoints(options, nJoints) == 0)
return false;
thetaMap = new int[nJoints];
if (getUsedJointsMap(options, nJoints, thetaMap) == 0)
return false;
//get the type of data to be dumped
if (getNumberDataToDump(options, nData) == 0)
return false;
dataToDump = new ConstString[nData];
if (getDataToDump(options, dataToDump, nData) == 0)
return false;
for (int i = 0; i < nData; i++)
fprintf(stderr, "%s \n", dataToDump[i].c_str());
//if (!fileOptions.check("d"))
// {
// fprintf(stderr, "Missing option 'd' in given config file\n");
// return 1;
// }
//Value& d = fileOptions.find("d");
//open remote control board
ddBoardOptions.put("device", "remote_controlboard");
ddDebugOptions.put("device", "debugInterfaceClient");
ConstString localPortName = name;
ConstString localDebugPortName = name;
localPortName = localPortName + "/controlBoardDumper/";
localDebugPortName = localPortName + "debug/";
//localPortName = localPortName + robot.asString();
localPortName = localPortName + part.asString();
localDebugPortName = localDebugPortName + part.asString();
ddBoardOptions.put("local", localPortName.c_str());
ddDebugOptions.put("local", localDebugPortName.c_str());
ConstString remotePortName = "/";
remotePortName = remotePortName + robot.asString();
remotePortName = remotePortName + "/";
remotePortName = remotePortName + part.asString();
ddBoardOptions.put("remote", remotePortName.c_str());
ddDebugOptions.put("remote", remotePortName.c_str());
fprintf(stderr, "%s", ddBoardOptions.toString().c_str());
// create a device
ddBoard.open(ddBoardOptions);
if (!ddBoard.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
ddDebug.open(ddDebugOptions);
if (!ddDebug.isValid()) {
printf("Debug Interface is mandatary to run this module. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
bool logToFile = false;
if (options.check("logToFile")) logToFile = true;
portPrefix = "/";
portPrefix= portPrefix + "controlBoardDumper/" + part.asString() + "/";
//boardDumperThread *myDumper = new boardDumperThread(&dd, rate, portPrefix, dataToDump[0]);
//myDumper->setThetaMap(thetaMap, nJoints);
myDumper = new boardDumperThread[nData];
for (int i = 0; i < nData; i++)
{
if (dataToDump[i] == "getEncoders")
if (ddBoard.view(ienc))
{
fprintf(stderr, "Initializing a getEncs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetEncs.setInterface(ienc);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetEncs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetEncs);
}
if (dataToDump[i] == "getEncoderSpeeds")
if (ddBoard.view(ienc))
{
fprintf(stderr, "Initializing a getSpeeds thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetSpeeds.setInterface(ienc);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
myGetSpeeds.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetSpeeds);
}
if (dataToDump[i] == "getEncoderAccelerations")
if (ddBoard.view(ienc))
{
fprintf(stderr, "Initializing a getAccs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetAccs.setInterface(ienc);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetAccs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetAccs);
}
if (dataToDump[i] == "getPidReferences")
if (ddBoard.view(ipid))
{
fprintf(stderr, "Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPidRefs.setInterface(ipid);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getPidReferences::The time stamp initalization interfaces was successfull! \n");
myGetPidRefs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPidRefs);
}
if (dataToDump[i] == "getPositionErrors")
if (ddBoard.view(ipid))
{
fprintf(stderr, "Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPosErrs.setInterface(ipid);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getPositionErrors::The time stamp initalization interfaces was successfull! \n");
myGetPosErrs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPosErrs);
}
if (dataToDump[i] == "getOutputs")
if (ddBoard.view(ipid))
{
fprintf(stderr, "Initializing a getOuts thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetOuts.setInterface(ipid);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getOutputs::The time stamp initalization interfaces was successfull! \n");
myGetOuts.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetOuts);
}
if (dataToDump[i] == "getCurrents")
if (ddBoard.view(iamp))
{
fprintf(stderr, "Initializing a getCurrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetCurrs.setInterface(iamp);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getCurrents::The time stamp initalization interfaces was successfull! \n");
myGetCurrs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetCurrs);
}
if (dataToDump[i] == "getTorques")
if (ddBoard.view(itrq))
{
fprintf(stderr, "Initializing a getTorques thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqs.setInterface(itrq);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getTorques::The time stamp initalization interfaces was successfull! \n");
myGetTrqs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqs);
}
if (dataToDump[i] == "getTorqueErrors")
if (ddBoard.view(itrq))
{
fprintf(stderr, "Initializing a getTorqueErrors thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqErrs.setInterface(itrq);
if (ddBoard.view(istmp))
{
fprintf(stderr, "getTorqueErrors::The time stamp initalization interfaces was successfull! \n");
myGetTrqErrs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqErrs);
}
if (dataToDump[i] == "getRotorPositions")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
fprintf(stderr, "Initializing a getRotorPosition thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorPoss.setInterface(idbg);
if (ddDebug.view(istmp))
{
fprintf(stderr, "getRotorPositions::The time stamp initalization interfaces was successfull! \n");
myGetRotorPoss.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorPoss);
}
else
{
printf("Debug Interface not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
}
if (dataToDump[i] == "getRotorSpeeds")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
fprintf(stderr, "Initializing a getRotorSpeed thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorVels.setInterface(idbg);
if (ddDebug.view(istmp))
{
fprintf(stderr, "getRotorSpeeds::The time stamp initalization interfaces was successfull! \n");
myGetRotorVels.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorVels);
}
else
{
printf("Debug Interface not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
}
if (dataToDump[i] == "getRotorAccelerations")
{
if (idbg==0 && ddDebug.isValid()) ddDebug.view(idbg);
if (idbg!=0)
{
fprintf(stderr, "Initializing a getRotorAcceleration thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetRotorAccs.setInterface(idbg);
if (ddDebug.view(istmp))
{
fprintf(stderr, "getRotorAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetRotorAccs.setStamp(istmp);
}
else
fprintf(stderr, "Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetRotorAccs);
}
else
{
printf("Debug Interface not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
}
}
Time::delay(1);
for (int i = 0; i < nData; i++)
myDumper[i].start();
return true;
}
bool OpenCVGrabber::open(Searchable & config) {
// Release any previously allocated resources, just in case
close();
m_saidSize = false;
m_saidResize = false;
m_transpose = false;
m_flip_x = false;
m_flip_y = false;
// Are we capturing from a file or a camera ?
ConstString file = config.check("movie", Value(""),
"if present, read from specified file rather than camera").asString();
fromFile = (file!="");
if (fromFile) {
// Try to open a capture object for the file
m_capture = (void*)cvCaptureFromAVI(file.c_str());
if (0 == m_capture) {
yError("Unable to open file '%s' for capture!", file.c_str());
return false;
}
// Should we loop?
m_loop = config.check("loop","if present, loop movie");
} else {
m_loop = false;
int camera_idx =
config.check("camera",
Value(CV_CAP_ANY),
"if present, read from camera identified by this index").asInt();
// Try to open a capture object for the first camera
m_capture = (void*)cvCaptureFromCAM(camera_idx);
if (0 == m_capture) {
yError("Unable to open camera for capture!");
return false;
}
if ( config.check("framerate","if present, specifies desired camera device framerate") ) {
double m_fps = config.check("framerate", Value(-1)).asDouble();
cvSetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FPS,m_fps);
}
if (config.check("flip_x", "if present, flip the image along the x-axis")) m_flip_x = true;
if (config.check("flip_y", "if present, flip the image along the y-axis")) m_flip_y = true;
if (config.check("transpose", "if present, rotate the image along of 90 degrees")) m_transpose = true;
}
// Extract the desired image size from the configuration if
// present, otherwise query the capture device
if (config.check("width","if present, specifies desired image width")) {
m_w = config.check("width", Value(-1)).asInt();
if (!fromFile && m_w>0) {
cvSetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FRAME_WIDTH, m_w);
}
} else {
m_w = (int)cvGetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FRAME_WIDTH);
}
if (config.check("height","if present, specifies desired image height")) {
m_h = config.check("height", Value(-1)).asInt();
if (!fromFile && m_h>0) {
cvSetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FRAME_HEIGHT, m_h);
}
} else {
m_h = (int)cvGetCaptureProperty((CvCapture*)m_capture,
CV_CAP_PROP_FRAME_HEIGHT);
}
// Ignore capture properties - they are unreliable
// yDebug("Capture properties: %ld x %ld pixels @ %lf frames/sec.", m_w, m_h, cvGetCaptureProperty(m_capture, CV_CAP_PROP_FPS));
yInfo("OpenCVGrabber opened");
// Success!
// save our configuration for future reference
m_config.fromString(config.toString());
return true;
}
bool VirtualAnalogWrapper::open(Searchable& config)
{
cout << config.toString().c_str() << endl << endl;
mIsVerbose = (config.check("verbose","if present, give detailed output"));
if (mIsVerbose) cout << "running with verbose output\n";
//thus thread period is useful for output port... this input port has callback so maybe can skip it (?)
//thread_period = prop.check("threadrate", 20, "thread rate in ms. for streaming encoder data").asInt();
std::cout << "Using VirtualAnalogServer\n";
if (!config.check("networks", "list of networks merged by this wrapper"))
{
cerr << "Error: missing networks parameters" << endl;
return false;
}
Bottle *networks=config.find("networks").asList();
mNSubdevs=networks->size();
mSubdevices.resize(mNSubdevs);
mChan2Board.resize(MAX_ENTRIES);
mChan2BAddr.resize(MAX_ENTRIES);
for (int i=0; i< MAX_ENTRIES; i++)
{
mChan2Board[i]=-1;
mChan2BAddr[i]=-1;
}
int totalJ=0;
for (int k=0; k<networks->size(); ++k)
{
Bottle parameters=config.findGroup(networks->get(k).asString().c_str());
if (parameters.size()!=5) // mapping joints using the paradigm: part from - to / network from - to
{
cerr << "Error: check network parameters in part description" << endl;
cerr << "--> I was expecting " << networks->get(k).asString().c_str() << " followed by four integers" << endl;
return false;
}
int map0=parameters.get(1).asInt();
int map1=parameters.get(2).asInt();
int map2=parameters.get(3).asInt();
int map3=parameters.get(4).asInt();
if (map0 >= MAX_ENTRIES || map1 >= MAX_ENTRIES || map2>= MAX_ENTRIES || map3>= MAX_ENTRIES ||
map0 <0 || map1 <0 || map2<0 || map3<0)
{
cerr << "Error: invalid map entries in networks section, failed initial check" << endl;
return false;
}
for (int j=map0; j<=map1; ++j)
{
mChan2Board[j]=k;
mChan2BAddr[j]=j-map0+map2;
}
if (!mSubdevices[k].configure(map2,map3,networks->get(k).asString().c_str()))
{
cerr << "configure of subdevice ret false" << endl;
return false;
}
totalJ+=map1-map0+1;
}
// Verify minimum set of parameters required
if(!config.check("robotName") ) // ?? qui dentro, da dove lo pesco ??
{
cout << "VirtualAnalogServer missing robot Name, check your configuration file!! Quitting\n";
return false;
}
std::string root_name;
std::string port_name = config.check("name",Value("controlboard"),"prefix for port names").asString().c_str();
std::string robot_name = config.find("robotName").asString().c_str();
root_name+="/";
root_name+=robot_name;
root_name+= "/joint_vsens" + port_name + ":i";
if (!mPortInputTorques.open(root_name.c_str()))
{
cerr << "can't open port " << root_name.c_str() << endl;
return false;
}
return true;
}
bool VirtualAnalogClient::open(Searchable& config)
{
yarp::os::Property prop;
prop.fromString(config.toString());
if( !( prop.check("local") && prop.find("local").isString() ) )
{
yError("VirtualAnalogClient: Missing required local string parameter");
return false;
}
m_local = prop.find("local").asString();
if( !( prop.check("remote") && prop.find("remote").isString() ) )
{
yError("VirtualAnalogClient: Missing required remote string parameter");
return false;
}
m_remote = prop.find("remote").asString();
yarp::os::ConstString carrier;
if( ( prop.check("carrier") && prop.find("carrier").isString() ) )
{
carrier = prop.find("carrier").asString();
}
else
{
carrier = "tcp";
}
if( !( prop.check("AxisName") && prop.find("AxisName").isList() ) )
{
yError("VirtualAnalogClient: Missing required AxisName list parameter");
return false;
}
Bottle * AxisNameBot = prop.find("AxisName").asList();
m_axisName.resize(AxisNameBot->size());
for(int jnt=0; jnt < AxisNameBot->size(); jnt++)
{
m_axisName[jnt] = AxisNameBot->get(jnt).asString();
}
// Handle type
m_axisType.resize(m_axisName.size());
if( ( prop.check("AxisType") && prop.find("AxisType").isList() ) )
{
Bottle * AxisTypeBot = prop.find("AxisType").asList();
for(int jnt=0; jnt < AxisTypeBot->size(); jnt++)
{
ConstString type = AxisTypeBot->get(jnt).asString();
if (type == "revolute")
{
m_axisType[jnt] = VOCAB_JOINTTYPE_REVOLUTE;
}
else if (type == "prismatic")
{
m_axisType[jnt] = VOCAB_JOINTTYPE_UNKNOWN;
}
else
{
yError() << "VirtualAnalogClient: unknown joint type " << type;
return false;
}
}
}
else
{
for(size_t jnt=0; jnt < m_axisType.size(); jnt++)
{
m_axisType[jnt] = VOCAB_JOINTTYPE_REVOLUTE;
}
}
if( !( prop.check("virtualAnalogSensorInteger") && prop.find("virtualAnalogSensorInteger").isInt() ) )
{
yError("VirtualAnalogClient: Missing required virtualAnalogSensorInteger int parameter");
return false;
}
m_virtualAnalogSensorInteger = prop.find("virtualAnalogSensorInteger").isInt();
// Resize buffer
measureBuffer.resize(m_axisName.size(),0.0);
// Open the port
bool ok = m_outputPort.open(m_local);
if( !ok )
{
yError() << "VirtualAnalogClient: impossible to open port " << m_local;
}
// Connect the port
bool autoconnect = true;
if( prop.check("autoconnect") )
{
if( prop.find("autoconnect").isBool() )
{
autoconnect = prop.find("autoconnect").asBool();
}
else
{
yError() << "VirtualAnalogClient: autoconnect option found, but is not a bool, exiting.";
}
}
if( autoconnect )
{
ok = Network::connect(m_local,m_remote,carrier);
if( !ok )
{
yError() << "VirtualAnalogClient: impossible to connect port " << m_local << " to " << m_remote << " with carrier " << carrier;
return false;
}
}
return true;
}
bool OpenCVGrabber::open(Searchable & config) {
m_saidSize = false;
m_saidResize = false;
m_transpose = false;
m_flip_x = false;
m_flip_y = false;
// Are we capturing from a file or a camera ?
std::string file = config.check("movie", Value(""),
"if present, read from specified file rather than camera").asString();
fromFile = (file!="");
if (fromFile) {
// Try to open a capture object for the file
m_cap.open(file.c_str());
if (!m_cap.isOpened()) {
yError("Unable to open file '%s' for capture!", file.c_str());
return false;
}
// Should we loop?
m_loop = config.check("loop","if present, loop movie");
} else {
m_loop = false;
int camera_idx =
config.check("camera",
Value(CV_CAP_ANY),
"if present, read from camera identified by this index").asInt32();
// Try to open a capture object for the first camera
m_cap.open(camera_idx);
if (!m_cap.isOpened()) {
yError("Unable to open camera for capture!");
return false;
}
else
{
yInfo("Capturing from camera: %d",camera_idx);
}
if ( config.check("framerate","if present, specifies desired camera device framerate") ) {
double m_fps = config.check("framerate", Value(-1)).asFloat64();
m_cap.set(CV_CAP_PROP_FPS, m_fps);
}
if (config.check("flip_x", "if present, flip the image along the x-axis")) m_flip_x = true;
if (config.check("flip_y", "if present, flip the image along the y-axis")) m_flip_y = true;
if (config.check("transpose", "if present, rotate the image along of 90 degrees")) m_transpose = true;
}
// Extract the desired image size from the configuration if
// present, otherwise query the capture device
if (config.check("width","if present, specifies desired image width")) {
m_w = config.check("width", Value(0)).asInt32();
if (!fromFile && m_w>0) {
m_cap.set(CV_CAP_PROP_FRAME_WIDTH, m_w);
}
} else {
m_w = (size_t)m_cap.get(CV_CAP_PROP_FRAME_WIDTH);
}
if (config.check("height","if present, specifies desired image height")) {
m_h = config.check("height", Value(0)).asInt32();
if (!fromFile && m_h>0) {
m_cap.set(CV_CAP_PROP_FRAME_HEIGHT, m_h);
}
} else {
m_w = (size_t)m_cap.get(CV_CAP_PROP_FRAME_HEIGHT);
}
// Ignore capture properties - they are unreliable
yInfo("OpenCVGrabber opened");
// Success!
// save our configuration for future reference
m_config.fromString(config.toString());
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 fromConfigFile(const ConstString& fname,Searchable& env, bool wipe=true) {
String searchPath =
env.check("CONFIG_PATH",
Value(""),
"path to search for config files").toString().c_str();
YARP_DEBUG(Logger::get(),
String("looking for ") + fname.c_str() + ", search path: " +
searchPath);
String pathPrefix("");
String txt;
bool ok = true;
if (!readFile(fname,txt,true)) {
ok = false;
SplitString ss(searchPath.c_str(),';');
for (int i=0; i<ss.size(); i++) {
String trial = ss.get(i);
trial += '/';
trial += fname;
YARP_DEBUG(Logger::get(),
String("looking for ") + fname + " as " +
trial.c_str());
txt = "";
if (readFile(trial.c_str(),txt,true)) {
ok = true;
pathPrefix = ss.get(i);
pathPrefix += '/';
break;
}
}
}
String path("");
String sfname = fname;
size_t index = sfname.rfind('/');
if (index==String::npos) {
index = sfname.rfind('\\');
}
if (index!=String::npos) {
path = sfname.substr(0,index);
}
if (!ok) {
YARP_ERROR(Logger::get(),String("cannot read from ") +
fname);
return false;
}
Property envExtended;
envExtended.fromString(env.toString());
if (path!="") {
if (searchPath.length()>0) {
searchPath += ";";
}
searchPath += pathPrefix;
searchPath += path;
envExtended.put("CONFIG_PATH",searchPath.c_str());
}
fromConfig(txt.c_str(),envExtended,wipe);
return true;
}
bool VirtualAnalogWrapper::open(Searchable& config)
{
yDebug() << config.toString().c_str();
mIsVerbose = (config.check("verbose","if present, give detailed output"));
if (mIsVerbose) yDebug() << "running with verbose output\n";
//thus thread period is useful for output port... this input port has callback so maybe can skip it (?)
//thread_period = prop.check("threadrate", 20, "thread rate in ms. for streaming encoder data").asInt();
yDebug() << "Using VirtualAnalogServer\n";
if (!config.check("networks", "list of networks merged by this wrapper"))
{
yError() << "VirtualAnalogWrapper: missing networks parameters";
return false;
}
Bottle *networks=config.find("networks").asList();
mNSubdevs=networks->size();
mSubdevices.resize(mNSubdevs);
mChan2Board.resize(MAX_ENTRIES);
mChan2BAddr.resize(MAX_ENTRIES);
for (int i=0; i< MAX_ENTRIES; i++)
{
mChan2Board[i]=-1;
mChan2BAddr[i]=-1;
}
int totalJ=0;
for (int k=0; k<networks->size(); ++k)
{
Bottle parameters=config.findGroup(networks->get(k).asString().c_str());
if (parameters.size()!=5) // mapping joints using the paradigm: part from - to / network from - to
{
yError() << "VirtualAnalogWrapper: check network parameters in part description"
<< " I was expecting " << networks->get(k).asString().c_str() << " followed by four integers";
return false;
}
int map0=parameters.get(1).asInt();
int map1=parameters.get(2).asInt();
int map2=parameters.get(3).asInt();
int map3=parameters.get(4).asInt();
if (map0 >= MAX_ENTRIES || map1 >= MAX_ENTRIES || map2>= MAX_ENTRIES || map3>= MAX_ENTRIES ||
map0 <0 || map1 <0 || map2<0 || map3<0)
{
yError() << "VirtualAnalogWrapper: invalid map entries in networks section, failed initial check";
return false;
}
for (int j=map0; j<=map1; ++j)
{
mChan2Board[j]=k;
mChan2BAddr[j]=j-map0+map2;
}
if (!mSubdevices[k].configure(map2,map3,networks->get(k).asString().c_str()))
{
yError() << "VirtualAnalogWrapper: configure of subdevice ret false";
return false;
}
totalJ+=map1-map0+1;
}
// Verify minimum set of parameters required
if(!config.check("robotName") ) // ?? qui dentro, da dove lo pesco ??
{
yError() << "VirtualAnalogWrapper: missing robotName, check your configuration file!";
return false;
}
if (config.check("deviceId"))
{
yError() << "VirtualAnalogWrapper: the parameter 'deviceId' has been deprecated, please use parameter 'name' instead. \n"
<< "e.g. In the VFT wrapper configuration files of your robot, replace '<param name=""deviceId""> left_arm </param>' \n"
<< "with '/icub/joint_vsens/left_arm:i' ";
return false;
}
std::string port_name = config.check("name",Value("controlboard"),"Virtual analog wrapper port name, e.g. /icub/joint_vsens/left_arm:i").asString().c_str();
std::string robot_name = config.find("robotName").asString().c_str();
if (!mPortInputTorques.open(port_name.c_str()))
{
yError() << "VirtualAnalogWrapper: can't open port " << port_name.c_str();
return false;
}
return true;
}
bool teo::CanBusControlboard::open(Searchable& config) {
std::string mode = config.check("mode",Value(DEFAULT_MODE),"position/velocity mode").asString();
int16_t ptModeMs = config.check("ptModeMs",Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
Bottle ids = config.findGroup("ids").tail(); //-- e.g. 15
Bottle trs = config.findGroup("trs").tail(); //-- e.g. 160
Bottle ks = config.findGroup("ks").tail(); //-- e.g. 0.0706
Bottle maxs = config.findGroup("maxs").tail(); //-- e.g. 360
Bottle mins = config.findGroup("mins").tail(); //-- e.g. -360
Bottle refAccelerations = config.findGroup("refAccelerations").tail(); //-- e.g. 0.575437
Bottle refSpeeds = config.findGroup("refSpeeds").tail(); //-- e.g. 737.2798
Bottle types = config.findGroup("types").tail(); //-- e.g. 15
//-- Initialize the CAN device.
Property canBusOptions;
canBusOptions.fromString(config.toString()); // canDevice, canBitrate
canBusOptions.put("device","CanBusHico");
canBusDevice.open(canBusOptions);
if( ! canBusDevice.isValid() ){
CD_ERROR("canBusDevice instantiation not worked.\n");
return false;
}
canBusDevice.view(iCanBus);
//-- Start the reading thread (required for checkMotionDoneRaw).
this->Thread::start();
//-- Populate the CAN nodes vector.
nodes.resize( ids.size() );
iControlLimitsRaw.resize( nodes.size() );
iControlModeRaw.resize( nodes.size() );
iEncodersTimedRaw.resize( nodes.size() );
iPositionControlRaw.resize( nodes.size() );
iPositionDirectRaw.resize( nodes.size() );
iTorqueControlRaw.resize( nodes.size() );
iVelocityControlRaw.resize( nodes.size() );
iCanBusSharer.resize( nodes.size() );
for(int i=0; i<nodes.size(); i++)
{
if(types.get(i).asString() == "")
CD_WARNING("Argument \"types\" empty at %d.\n",i);
//-- Create CAN node objects with a pointer to the CAN device, its id and tr (these are locally stored parameters).
Property options;
options.put("device",types.get(i).asString()); //-- "TechnosoftIpos", "LacqueyFetch"
options.put("canId",ids.get(i).asInt());
options.put("tr",trs.get(i).asDouble());
options.put("min",mins.get(i).asDouble());
options.put("max",maxs.get(i).asDouble());
options.put("k",ks.get(i).asDouble());
options.put("refAcceleration",refAccelerations.get(i).asDouble());
options.put("refSpeed",refSpeeds.get(i).asDouble());
options.put("ptModeMs",ptModeMs);
PolyDriver* driver = new PolyDriver(options);
//-- Fill a map entry ( drivers.size() if before push_back, otherwise do drivers.size()-1).
//-- Just "i" if resize already performed.
idxFromCanId[ ids.get(i).asInt() ] = i;
//-- Push the motor driver on to the vectors.
nodes[i] = driver;
driver->view( iControlLimitsRaw[i] );
driver->view( iControlModeRaw[i] );
driver->view( iEncodersTimedRaw[i] );
driver->view( iPositionControlRaw[i] );
driver->view( iPositionDirectRaw[i] );
driver->view( iTorqueControlRaw[i] );
driver->view( iVelocityControlRaw[i] );
driver->view( iCanBusSharer[i] );
//-- Associate absolute encoders to motor drivers
if( types.get(i).asString() == "CuiAbsolute" ) {
int driverCanId = ids.get(i).asInt() - 100;
iCanBusSharer[ idxFromCanId[driverCanId] ]->setIEncodersTimedRawExternal( iEncodersTimedRaw[i] );
}
//-- Aditionally sets initial parameters on physical motor drivers.
iCanBusSharer[i]->setCanBusPtr( iCanBus );
}
//-- Set all motor drivers to mode.
if( mode=="position") {
if( ! this->setPositionMode() )
return false;
} else if( mode=="velocity") {
if( ! this->setVelocityMode() )
return false;
} else if( mode=="torque") {
if( ! this->setTorqueMode() )
return false;
} else {
CD_ERROR("Not prepared for initializing in mode %s.\n",mode.c_str());
return false;
}
//-- Check the status of each driver.
std::vector<int> tmp( nodes.size() );
this->getControlModes( tmp.data() );
//-- Initialize the drivers: start (0.1) ready (0.1) on (2) enable. Wait between each step.
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->start() )
return false;
}
yarp::os::Time::delay(0.1);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->readyToSwitchOn() )
return false;
}
yarp::os::Time::delay(0.1);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->switchOn() )
return false;
}
yarp::os::Time::delay(2);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->enable() )
return false;
}
if( config.check("home") ) {
CD_DEBUG("Moving motors to zero.\n");
for(int i=0; i<nodes.size(); i++)
{
if ( ! iPositionControlRaw[i]->positionMoveRaw(0,0) )
return false;
}
for(int i=0; i<nodes.size(); i++)
{
bool motionDone = false;
while( ! motionDone ) {
yarp::os::Time::delay(0.5); //-- [s]
CD_DEBUG("Moving %d to zero...\n",i);
if( ! iPositionControlRaw[i]->checkMotionDoneRaw(0,&motionDone) )
return false;
}
}
CD_DEBUG("Moved motors to zero.\n");
}
if( config.check("reset") ) {
CD_DEBUG("Forcing encoders to zero.\n");
if ( ! this->resetEncoders() )
return false;
}
return true;
}
