Skip to content

Commit

Permalink
add cmdParser to pImpl
Browse files Browse the repository at this point in the history
  • Loading branch information
@lrapetti
lrapetti committed Nov 26, 2019
1 parent 7719914 commit bd68c04
Showing 1 changed file with 100 additions and 83 deletions.
183 changes: 100 additions & 83 deletions devices/HumanStateProvider/HumanStateProvider.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,79 +132,6 @@ struct WearableStorage
jointSensorsMap;
};

class CmdParser : public yarp::os::PortReader
{

public:
std::atomic<bool> cmdStatus{false};
std::atomic<bool> cmdErease{false};
std::string parentLinkName;
std::string childLinkName;

bool read(yarp::os::ConnectionReader& connection) override
{
this->parentLinkName = "";
this->childLinkName = "";

yarp::os::Bottle command, response;
if (command.read(connection)) {

if (command.get(0).asString() == "help") {
response.addString("Enter <calibrate> to apply a secondary calibration for all the links");
response.addString("Enter <calibrate <linkName>> to apply a secondary calibration for the given link");
response.addString("Enter <calibrate <parentLinkName> <childLinkName>> to apply a secondary calibration for the given chain");
response.addString("Enter <reset <linkName>> to remove secondary calibration for the given link");
response.addString("Enter <reset> to remove all the secondary calibrations");
}
else if (command.get(0).asString() == "calibrate" && !command.get(1).isNull() && !command.get(2).isNull()) {
this->parentLinkName = command.get(1).asString();
this->childLinkName = command.get(2).asString();
response.addString("Entered command <calibrate> is correct, setting the offset for the chain from " + this->parentLinkName + " to " + this->childLinkName);
this->cmdStatus = true;
}
else if (command.get(0).asString() == "calibrate" && !command.get(1).isNull()) {
this->parentLinkName = command.get(1).asString();
response.addString("Entered command <calibrate> is correct, setting the offset calibration for the link " + this->parentLinkName);
this->cmdStatus = true;
}
else if (command.get(0).asString() == "calibrate") {
response.addString("Entered command <calibrate> is missing the linkName. Setting the offset calibartion for all the links");
this->cmdStatus = true;
}
else if (command.get(0).asString() == "reset" && command.get(1).isNull()) {
response.addString("Entered command <reset> is correct, removing all the secondary calibrations ");
this->cmdStatus = true;
this->cmdErease = true;
}
else if (command.get(0).asString() == "reset" && !command.get(1).isNull()) {
this->parentLinkName = command.get(1).asString();
response.addString("Entered command <reset> is correct, removing the secondaty calibration for the link " + this->parentLinkName);
this->cmdStatus = true;
this->cmdErease = true;
}
else {
response.addString(
"Entered command is incorrect. Enter help to know available commands");
}
}
else {
this->cmdStatus = false;
this->cmdErease = false;
return false;
}

yarp::os::ConnectionWriter* reply = connection.getWriter();

if (reply != NULL) {
response.write(*reply);
}
else
return false;

return true;
}
};

class HumanStateProvider::impl
{
public:
Expand All @@ -218,7 +145,8 @@ class HumanStateProvider::impl
mutable std::mutex mutex;

// Rpc
CmdParser commandPro;
class CmdParser;
std::unique_ptr<CmdParser> commandPro;
yarp::os::RpcServer rpcPort;

// Wearable variables
Expand Down Expand Up @@ -340,8 +268,97 @@ class HumanStateProvider::impl
iDynTree::VectorDynSize jointVelocities,
iDynTree::Twist baseVelocity,
std::unordered_map<std::string, iDynTree::Vector3>& linkAngularVelocityError);

// constructor
impl();
};

// ===============
// RPC PORT PARSER
// ===============

class HumanStateProvider::impl::CmdParser : public yarp::os::PortReader
{

public:
std::atomic<bool> cmdStatus{false};
std::atomic<bool> cmdErease{false};
std::string parentLinkName;
std::string childLinkName;

bool read(yarp::os::ConnectionReader& connection) override
{
this->parentLinkName = "";
this->childLinkName = "";

yarp::os::Bottle command, response;
if (command.read(connection)) {

if (command.get(0).asString() == "help") {
response.addString("Enter <calibrate> to apply a secondary calibration for all the links");
response.addString("Enter <calibrate <linkName>> to apply a secondary calibration for the given link");
response.addString("Enter <calibrate <parentLinkName> <childLinkName>> to apply a secondary calibration for the given chain");
response.addString("Enter <reset <linkName>> to remove secondary calibration for the given link");
response.addString("Enter <reset> to remove all the secondary calibrations");
}
else if (command.get(0).asString() == "calibrate" && !command.get(1).isNull() && !command.get(2).isNull()) {
this->parentLinkName = command.get(1).asString();
this->childLinkName = command.get(2).asString();
response.addString("Entered command <calibrate> is correct, setting the offset for the chain from " + this->parentLinkName + " to " + this->childLinkName);
this->cmdStatus = true;
}
else if (command.get(0).asString() == "calibrate" && !command.get(1).isNull()) {
this->parentLinkName = command.get(1).asString();
response.addString("Entered command <calibrate> is correct, setting the offset calibration for the link " + this->parentLinkName);
this->cmdStatus = true;
}
else if (command.get(0).asString() == "calibrate") {
response.addString("Entered command <calibrate> is missing the linkName. Setting the offset calibartion for all the links");
this->cmdStatus = true;
}
else if (command.get(0).asString() == "reset" && command.get(1).isNull()) {
response.addString("Entered command <reset> is correct, removing all the secondary calibrations ");
this->cmdStatus = true;
this->cmdErease = true;
}
else if (command.get(0).asString() == "reset" && !command.get(1).isNull()) {
this->parentLinkName = command.get(1).asString();
response.addString("Entered command <reset> is correct, removing the secondaty calibration for the link " + this->parentLinkName);
this->cmdStatus = true;
this->cmdErease = true;
}
else {
response.addString(
"Entered command is incorrect. Enter help to know available commands");
}
}
else {
this->cmdStatus = false;
this->cmdErease = false;
return false;
}

yarp::os::ConnectionWriter* reply = connection.getWriter();

if (reply != NULL) {
response.write(*reply);
}
else
return false;

return true;
}
};

// ===========
// CONSTRUCTOR
// ===========

HumanStateProvider::impl::impl()
: commandPro(new CmdParser())
{}


// =========================
// HUMANSTATEPROVIDER DEVICE
// =========================
Expand Down Expand Up @@ -1015,7 +1032,7 @@ bool HumanStateProvider::open(yarp::os::Searchable& config)
}

// Set rpc port reader
pImpl->rpcPort.setReader(pImpl->commandPro);
pImpl->rpcPort.setReader(*pImpl->commandPro);

return true;
}
Expand Down Expand Up @@ -1137,14 +1154,14 @@ void HumanStateProvider::run()
}

// Check for rpc command status
if (pImpl->commandPro.cmdStatus) {
std::string linkName = pImpl->commandPro.parentLinkName;
std::string childLinkName = pImpl->commandPro.childLinkName;
if (pImpl->commandPro.cmdErease && linkName == "")
if (pImpl->commandPro->cmdStatus) {
std::string linkName = pImpl->commandPro->parentLinkName;
std::string childLinkName = pImpl->commandPro->childLinkName;
if (pImpl->commandPro->cmdErease && linkName == "")
{
pImpl->secondaryCalibrationRotations.clear();
}
else if (!pImpl->commandPro.cmdErease && linkName == "")
else if (!pImpl->commandPro->cmdErease && linkName == "")
{
iDynTree::VectorDynSize jointPos;
jointPos.resize(kindyncomputations->getNrOfDegreesOfFreedom());
Expand Down Expand Up @@ -1173,7 +1190,7 @@ void HumanStateProvider::run()
else if (childLinkName != "" && pImpl->wearableStorage.modelToWearable_LinkName.find(childLinkName) == pImpl->wearableStorage.modelToWearable_LinkName.end()) {
yWarning() << LogPrefix << "link " << childLinkName << " choosen for secondaty calibration is not valid";
}
else if (pImpl->commandPro.cmdErease) {
else if (pImpl->commandPro->cmdErease) {
pImpl->secondaryCalibrationRotations.erase(linkName);
}
else if (childLinkName == "") {
Expand Down Expand Up @@ -1240,8 +1257,8 @@ void HumanStateProvider::run()
}

// Set rpc command status to false
pImpl->commandPro.cmdStatus = false;
pImpl->commandPro.cmdErease = false;
pImpl->commandPro->cmdStatus = false;
pImpl->commandPro->cmdErease = false;

// compute the inverse kinematic errors (currently the result is unused, but it may be used for
// evaluating the IK performance)
Expand Down

0 comments on commit bd68c04

Please sign in to comment.