22#include <unordered_set>
32 dbg <<
"(name = \"" << t.
name() <<
"\"";
34 dbg <<
", params = [";
39 dbg <<
", devices = [";
149 for (
int i = 0; i < externalDevices.size(); i++) {
151 yCInfo(YRI_ROBOT) <<
"- " << externalDevice->
key;
160 for (
int i = 0; i < externalDevices.size(); i++) {
162 if (
name == externalDevice->
key) {
174 deviceFound.
poly =
nullptr;
175 deviceFound.
key =
"";
185 for (
int i = 0; i < externalDevices.size(); i++) {
187 if (
name == externalDevice->
key) {
188 deviceFound = *externalDevice;
197 std::unordered_set<std::string> externalDevicesNames;
199 for (
int i = 0; i < externalDevicesList.
size(); i++) {
201 externalDevicesNames.insert(externalDevice->
key);
205 if (externalDevicesNames.find(
device.
name()) != externalDevicesNames.end()) {
206 yCError(YRI_ROBOT) <<
"Device name " <<
device.
name() <<
" is used for both an internal and external device.";
218 pcfg.
put(
"device",
"robotDescriptionStorage");
219 pcfg.
put(
"name",
"yriDescriptionStorage");
220 m_ddstorage.
open(pcfg);
221 m_ddstorage.view(m_istorage);
226 if (showDeviceParameters)
232 yCInfo(YRI_ROBOT) <<
"Opening device" <<
device.
name() <<
". Use option `show-device-parameters` for additional info.";
247 std::vector<std::string> stp;
253 pddrv->
view(dparams);
261 yCWarning(YRI_ROBOT) <<
"Device" <<
device.
name() <<
"does not derive from IDeviceDriverParams.";
264 if (!pddrv || scfg.empty())
266 yCWarning(YRI_ROBOT) <<
"Unable to get device" <<
device.
name() <<
"configuration. yarprobotinterface will continue, but some features for inspecting the device parameters will be disabled.";
267 yCDebug(YRI_ROBOT) <<
"It is recommended that devices used by yarprobinterface implement the `yarp::dev::IDeviceDriverParams` interface.";
268 yCDebug(YRI_ROBOT) <<
"See yarprobotinterface documentation page.";
277 yCError(YRI_ROBOT) <<
"Unable to register device" <<
device.
name() <<
"in robotDescriptionStorage";
286 std::vector<yarp::dev::DeviceDescription> ll;
287 m_istorage->getAllDevices(ll);
288 std::ostringstream oss;
289 for (
auto& it_device : ll)
291 oss <<
"- name:" << it_device.device_name <<
", type:" << it_device.device_type <<
"\n";
293 yCInfo(YRI_ROBOT) <<
"List of opened devices:\n"
294 << oss.str() <<
"End of list";
300 yCWarning(YRI_ROBOT) <<
"There was some problem opening one or more devices. Please check the log and your configuration";
326 yCInfo(YRI_ROBOT) <<
"All devices closed.";
328 yCWarning(YRI_ROBOT) <<
"There was some problem closing one or more devices. Please check the log and your configuration";
331 if (m_ddstorage.isValid())
341 std::vector<unsigned int> levels;
348 if (action.phase() == phase) {
349 levels.push_back(action.level());
354 std::sort(levels.begin(), levels.end());
355 auto it = std::unique(levels.begin(), levels.end());
356 levels.resize(it - levels.begin());
364 std::vector<std::pair<yarp::robotinterface::Device, yarp::robotinterface::Action>> actions;
371 if (action.phase() == phase && action.level() == level) {
372 actions.emplace_back(
device, action);
395 if (!hasDeviceIncludingExternal(targetDeviceName)) {
396 yCError(YRI_ROBOT) <<
"Target device" << targetDeviceName <<
"does not exist.";
397 yCError(YRI_ROBOT) <<
"Available target devices are:";
398 PrintDevicesIncludingExternal();
414 if (
device.
type() ==
"robotDescription_nws_yarp")
418 drv_list.
push(&m_ddstorage, yriDescriptionStorageName.c_str());
422 yCError(YRI_ROBOT) <<
"cannot attach robotDescription_nws_yarp to yriDescriptionStorage";
448 std::string targetNetwork =
"...";
453 if (!hasDeviceIncludingExternal(targetDeviceName)) {
454 yCError(YRI_ROBOT) <<
"Target device" << targetDeviceName <<
"(network =" << targetNetwork <<
") does not exist.";
455 yCError(YRI_ROBOT) <<
"Available target devices are:";
456 PrintDevicesIncludingExternal();
464 drivers.
push(targetDevice.
poly, targetNetwork.c_str());
473 for (
int i = 0; i < externalDevices.size(); i++) {
475 drivers.
push(externalDevice->
poly,
"all");
483 for (
size_t i = 0; i < targetNetworks.
size(); ++i) {
484 std::string targetNetwork = targetNetworks.
get(i).
toString();
491 if (!hasDeviceIncludingExternal(targetDeviceName)) {
492 yCError(YRI_ROBOT) <<
"Target device" << targetDeviceName <<
"(network =" << targetNetwork <<
") does not exist.";
493 yCError(YRI_ROBOT) <<
"Available target devices are:";
494 PrintDevicesIncludingExternal();
502 drivers.
push(targetDevice.
poly, targetNetwork.c_str());
514 if (!drivers.
size()) {
551 if (!hasDeviceIncludingExternal(targetDeviceName)) {
552 yCError(YRI_ROBOT) <<
"Target device" << targetDeviceName <<
"does not exist.";
553 yCError(YRI_ROBOT) <<
"Available target devices are:";
554 PrintDevicesIncludingExternal();
602 if (&other !=
this) {
604 mPriv->build = other.mPriv->
build;
608 mPriv->dryrun = other.mPriv->
dryrun;
612 mPriv->devices.clear();
613 mPriv->devices = other.mPriv->
devices;
615 mPriv->params.clear();
616 mPriv->params = other.mPriv->
params;
639 return mPriv->portprefix;
644 for (
auto& device : devices()) {
648 device.params().push_back(
Param(
"verbose",
"1"));
655 for (
auto& device : devices()) {
659 device.params().push_back(
Param(
"allow-deprecated-devices",
"1"));
666 mPriv->showDeviceParameters = show;
671 mPriv->dryrun = dryrun;
676 mPriv->reverseShutdownActionOrder = reverseShutdownActionOrder;
681 return mPriv->params;
686 return mPriv->devices;
691 return *mPriv->findDevice(name);
706 return mPriv->portprefix;
711 return mPriv->params;
716 return mPriv->devices;
721 return mPriv->hasDevice(name);
726 return *mPriv->findDevice(name);
731 yCInfo(YRI_ROBOT) <<
"Interrupt received. Stopping all running threads.";
735 for (
auto& device : devices()) {
742 bool nameConflict = mPriv->checkForNamingConflictsInExternalDevices(list);
747 mPriv->externalDevices = list;
755 mPriv->currentPhase = phase;
756 mPriv->currentLevel = 0;
760 if (!mPriv->openDevices()) {
761 yCError(YRI_ROBOT) <<
"One or more devices failed opening... see previous log messages for more info";
762 if (!mPriv->closeDevices()) {
763 yCError(YRI_ROBOT) <<
"One or more devices failed closing";
771 if (mPriv->getLevels(phase).size() != 0) {
784 for (
auto& device : devices()) {
785 device.joinThreads();
789 std::vector<unsigned int> levels = mPriv->getLevels(phase);
794 std::reverse(levels.begin(), levels.end());
798 for (
unsigned int level : levels) {
801 mPriv->currentLevel = level;
805 if (mPriv->currentPhase != phase) {
810 std::vector<std::pair<Device, Action>> actions = mPriv->getActions(phase, level);
812 for (
auto& ait : actions) {
814 Device& device = ait.first;
815 Action& action = ait.second;
819 if (mPriv->currentPhase != phase) {
826 switch (action.
type()) {
828 if (!mPriv->configure(device, action.
params())) {
829 yCError(YRI_ROBOT) <<
"Cannot run configure action on device" << device.
name();
834 if (!mPriv->calibrate(device, action.
params())) {
835 yCError(YRI_ROBOT) <<
"Cannot run calibrate action on device" << device.
name();
840 if (!mPriv->attach(device, action.
params())) {
841 yCError(YRI_ROBOT) <<
"Cannot run attach action on device" << device.
name();
846 if (!mPriv->abort(device, action.
params())) {
847 yCError(YRI_ROBOT) <<
"Cannot run abort action on device" << device.
name();
852 if (!mPriv->detach(device, action.
params())) {
853 yCError(YRI_ROBOT) <<
"Cannot run detach action on device" << device.
name();
858 if (!mPriv->park(device, action.
params())) {
859 yCError(YRI_ROBOT) <<
"Cannot run park action on device" << device.
name();
864 if (!mPriv->custom(device, action.
params())) {
865 yCError(YRI_ROBOT) <<
"Cannot run custom action on device" << device.
name();
876 yCInfo(YRI_ROBOT) <<
"All actions for action level" << level <<
"of" <<
ActionPhaseToString(phase) <<
"phase started. Waiting for unfinished actions.";
879 for (
auto& device : devices()) {
880 device.joinThreads();
888 yCWarning(YRI_ROBOT) <<
"There was some problem running actions for" <<
ActionPhaseToString(phase) <<
"phase . Please check the log and your configuration";
892 if (!mPriv->closeDevices()) {
893 yCError(YRI_ROBOT) <<
"One or more devices failed closing";
905 return mPriv->currentPhase;
910 return mPriv->currentLevel;
#define YARP_FIXME_NOTIMPLEMENTED(what)
yarp::os::LogStream operator<<(yarp::os::LogStream dbg, const yarp::robotinterface::Robot &t)
std::string device_configuration
configuration parameters of the device
std::string device_type
type of the device
std::string device_name
name of the device
bool view(T *&x)
Get an interface to the device driver.
An interface for the management of the parameters of a DeviceDriver.
virtual std::vector< std::string > getListOfParams() const =0
Return a list of all params used by the device.
virtual std::string getConfiguration() const =0
Return the configuration of the device.
This interface allows users to retrieve a list which contains the names and the types of the currentl...
void push(PolyDriver *p, const char *k)
A container for a device driver.
A simple collection of objects that can be described and transmitted in a portable way.
size_type size() const
Gets the number of elements in the bottle.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
A class for storing options and configuration information.
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
A single value (typically within a Bottle).
virtual Bottle * asList() const
Get list value.
std::string toString() const override
Return a standard text representation of the content of the object.
void fromString(const char *str)
Set value to correspond to a textual representation.
bool attach(const yarp::dev::PolyDriverList &drivers) const
bool park(const yarp::dev::PolyDriverDescriptor &target) const
yarp::dev::PolyDriver * driver() const
bool calibrate(const yarp::dev::PolyDriverDescriptor &target) const
bool showDeviceParameters
unsigned int currentLevel
bool detach(const Device &device, const ParamList ¶ms)
std::vector< unsigned int > getLevels(ActionPhase phase) const
yarp::dev::IRobotDescription * m_istorage
Device * findDevice(const std::string &name)
std::string yriDescriptionStorageName
bool reverseShutdownActionOrder
bool park(const Device &device, const ParamList ¶ms)
bool custom(const Device &device, const ParamList ¶ms)
bool hasDevice(const std::string &name) const
void PrintDevicesIncludingExternal() const
bool calibrate(const Device &device, const ParamList ¶ms)
bool hasDeviceIncludingExternal(const std::string &name) const
bool attach(const Device &device, const ParamList ¶ms)
bool checkForNamingConflictsInExternalDevices(const yarp::dev::PolyDriverList &newExternalDevicesList)
yarp::dev::PolyDriver m_ddstorage
yarp::robotinterface::ActionPhase currentPhase
yarp::dev::PolyDriverDescriptor findDeviceIncludingExternal(const std::string &name)
bool configure(const Device &device, const ParamList ¶ms)
std::vector< std::pair< Device, Action > > getActions(ActionPhase phase, unsigned int level) const
yarp::dev::PolyDriverList externalDevices
bool abort(const Device &device, const ParamList ¶ms)
bool hasParam(const std::string &name) const
void setAllowDeprecatedDevices(bool allowDeprecatedDevices)
bool hasDevice(const std::string &name) const
void setShowDeviceParameters(bool show)
yarp::robotinterface::ActionPhase currentPhase() const
std::string & portprefix()
void setVerbose(bool verbose)
bool enterPhase(yarp::robotinterface::ActionPhase phase)
bool setExternalDevices(const yarp::dev::PolyDriverList &list)
void setReverseShutdownActionOrder(bool reverseShutdownActionOrder)
Robot & operator=(const Robot &other)
std::string findParam(const std::string &name) const
Device & device(const std::string &name)
void setDryRun(bool dryrun)
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
std::string ActionTypeToString(robotinterface::ActionType actiontype)
bool hasParam(const robotinterface::ParamList &list, const std::string &name)
std::vector< robotinterface::Param > ParamList
std::string findParam(const robotinterface::ParamList &list, const std::string &name)
std::vector< robotinterface::Device > DeviceList
std::string ActionPhaseToString(robotinterface::ActionPhase actionphase)