iCub-main
WholeBodyPlayerModule.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
10 #include <algorithm>
11 #include <yarp/dev/GenericVocabs.h>
12 
13 using namespace yarp::os;
14 using namespace yarp::dev;
15 using namespace std;
16 
17 
18 static const std::vector<std::string> partsVec {"head","torso","left_arm","right_arm","left_leg","right_leg"};
19 
21  return 0.001;
22 }
23 
25 
26  for (auto& rep : m_replayerVec){
27  if (rep.m_replayPort->m_state == state::fatal_error) {
28  yError()<<"wholeBodyPlayer: the port"<<rep.m_replayPort->getName()<<"is closed because something went wrong.. closing";
29  return false;
30  } else if (rep.m_replayPort->m_state == state::error) {
31  bool ok{true};
32  response.clear();
33  // pause
34  ok &= m_rpcPort.write(reqPause, response);
35  if (!ok || response.get(0).asVocab32() != VOCAB_OK) {
36  yError()<<"wholeBodyPlayer: the port"<<rep.m_replayPort->getName()<<"is closed because the pause request failed.. closing";
37  return false;
38  }
39 
40  // position move fallback
41  ok &= rep.m_replayPort->positionMoveFallback();
42  if (!ok) {
43  yError()<<"wholeBodyPlayer: the port"<<rep.m_replayPort->getName()<<"is closed because the fallback failed.. closing";
44  return false;
45  }
46 
47  // start
48  response.clear();
49  ok &= m_rpcPort.write(reqPlay, response);
50  if (!ok || response.get(0).asVocab32() != VOCAB_OK) {
51  yError()<<"wholeBodyPlayer: the port"<<rep.m_replayPort->getName()<<"is closed because the start request failed.. closing";
52  return false;
53  }
54  rep.m_replayPort->m_state = state::ok;
55  }
56  }
57 
58  return true;
59 }
60 
61 bool WholeBodyPlayerModule::configure(yarp::os::ResourceFinder& rf) {
62  auto robot = rf.check("robot",Value("icub")).asString();
63  auto name = rf.check("name",Value("wholeBodyPlayerModule")).asString();
64 
65  auto partsBot = rf.find("parts").asList();
66  if (!partsBot || partsBot->isNull())
67  {
68  yError()<<"wholeBodyPlayerModule: missing parts parameter, please specify as list";
69  return false;
70  }
71 
72  m_replayerVec.resize(partsBot->size());
73 
74  for (size_t i = 0; i<partsBot->size(); i++) {
75  auto partStr = partsBot->get(i).asString();
76  if(partsVec.end() == std::find(partsVec.begin(), partsVec.end(), partStr))
77  {
78  yError()<<"wholeBodyPlayerModule: the part"<<partStr<<"is not available";
79  return false;
80  }
81  if (!m_replayerVec[i].open(robot, partStr, name))
82  {
83  yError()<<"wholeBodyPlayerModule: failed to open one replayer.. closing.";
84  return false;
85  }
86  }
87 
88  if (!m_rpcPort.open("/"+name+"/rpc:o")) {
89  yError()<<"wholeBodyPlayerModule: failed to open"<<m_rpcPort.getName();
90  return false;
91  }
92 
93  if (!Network::connect(m_rpcPort.getName(), "/yarpdataplayer/rpc:i")) {
94  yError()<<"wholeBodyPlayerModule: failed to connect to the yarpdataplayer, is it running?";
95  return false;
96  }
97 
98  return true;
99 }
100 
101 bool WholeBodyPlayerModule::respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply) {
102  return true;
103 }
104 
106  for (auto& rep : m_replayerVec){
107  rep.m_replayPort->interrupt();
108  }
109  m_rpcPort.interrupt();
110  return true;
111 }
112 
114  for (auto& rep : m_replayerVec){
115  rep.close();
116  }
117  m_rpcPort.close();
118  return true;
119 }
WholeBodyPlayerModule::updateModule
bool updateModule() override
Definition: WholeBodyPlayerModule.cpp:24
yarp::dev
Definition: DebugInterfaces.h:52
state::error
@ error
WholeBodyPlayerModule::getPeriod
double getPeriod() override
Definition: WholeBodyPlayerModule.cpp:20
state::ok
@ ok
WholeBodyPlayerModule::configure
bool configure(yarp::os::ResourceFinder &rf) override
Definition: WholeBodyPlayerModule.cpp:61
WholeBodyPlayerModule.h
state::fatal_error
@ fatal_error
partsVec
static const std::vector< std::string > partsVec
Definition: WholeBodyPlayerModule.cpp:18
WholeBodyPlayerModule::close
bool close() override
Definition: WholeBodyPlayerModule.cpp:113
WholeBodyPlayerModule::interruptModule
bool interruptModule() override
Definition: WholeBodyPlayerModule.cpp:105
WholeBodyPlayerModule::respond
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply) override
Definition: WholeBodyPlayerModule.cpp:101