Skip to content

Commit

Permalink
Update PR changes and rpc reply handling
Browse files Browse the repository at this point in the history
  • Loading branch information
@yeshasvitirupachuri
yeshasvitirupachuri committed Mar 30, 2019
1 parent 22c7f0c commit f796908
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 30 deletions.
7 changes: 1 addition & 6 deletions plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#include <yarp/os/Bottle.h>
#include <yarp/sig/Vector.h>
#include <yarp/os/Thread.h>
#include <yarp/os/Time.h>
#include <yarp/os/Vocab.h>
#include <yarp/os/LogStream.h>

Expand Down Expand Up @@ -49,6 +48,7 @@ private:
// variable for operation mode
// single wrench or multiple wrenches
std::string m_mode;
std::string m_message;
};


Expand All @@ -69,8 +69,6 @@ public:
/// \brief Robot name that will be used to open rpc port
std::string robotName;

double timeIni;

protected:
// Inherited
void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
Expand All @@ -82,7 +80,6 @@ private:

physics::ModelPtr m_myModel;


/// \brief Link the plugin is attached to
physics::LinkPtr m_onLink;

Expand All @@ -96,8 +93,6 @@ private:
msgs::Visual m_visualMsg;

bool m_newCommand;
bool m_setMessage;

};

}
Expand Down
52 changes: 38 additions & 14 deletions plugins/externalwrench/src/ApplyExternalWrench.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,12 +116,11 @@ void RPCServerThread::run()
this->m_reply.addString ( "Note: The reference frame is the base/root robot frame with x pointing backwards and z upwards.");
this->m_rpcPort.reply ( this->m_reply );
} else{
if(command.get(0).isString() \
&& ( command.get ( 1 ).isDouble() || command.get ( 1 ).isInt() ) && ( command.get ( 2 ).isDouble() || command.get ( 2 ).isInt() ) && ( command.get ( 3 ).isDouble() || command.get ( 3 ).isInt() ) \
&& ( command.get ( 4 ).isDouble() || command.get ( 4 ).isInt() ) && ( command.get ( 5 ).isDouble() || command.get ( 5 ).isInt() ) && ( command.get ( 6 ).isDouble() || command.get ( 6 ).isInt() ) \
&& ( command.get ( 7 ).isDouble() || command.get ( 7 ).isInt() ) ) {
this->m_reply.addString ( "[ACK] Correct command format" );
this->m_rpcPort.reply ( m_reply );
if((command.size() == 8) && (command.get(0).isString() \
&& ( command.get ( 1 ).isDouble() || command.get ( 1 ).isInt() ) && ( command.get ( 2 ).isDouble() || command.get ( 2 ).isInt() ) && ( command.get ( 3 ).isDouble() || command.get ( 3 ).isInt() ) \
&& ( command.get ( 4 ).isDouble() || command.get ( 4 ).isInt() ) && ( command.get ( 5 ).isDouble() || command.get ( 5 ).isInt() ) && ( command.get ( 6 ).isDouble() || command.get ( 6 ).isInt() ) \
&& ( command.get ( 7 ).isDouble() || command.get ( 7 ).isInt() )) ) {
this->m_message = "[ACK] Correct command format";
m_lock.lock();
// new-command flag
command.addInt(1);
Expand All @@ -130,10 +129,13 @@ void RPCServerThread::run()

if (this->m_mode == "single") {

// Delete the previous wrenchs
// Delete the previous wrenches
if (wrenchesVectorPtr->size() != 0) {
boost::shared_ptr<ExternalWrench> wrench = wrenchesVectorPtr->at(0);
wrench->deleteWrench();
for (int i = 0; i < wrenchesVectorPtr->size(); i++)
{
boost::shared_ptr<ExternalWrench> wrench = wrenchesVectorPtr->at(i);
wrench->deleteWrench();
}
wrenchesVectorPtr->clear();
}

Expand All @@ -143,20 +145,42 @@ void RPCServerThread::run()
boost::shared_ptr<ExternalWrench> newWrench(new ExternalWrench);
if(newWrench->setWrench(m_robotModel, m_cmd))
{
this->m_message = this->m_message + " and " + command.get(0).asString() + " link found in the model" ;
this->m_reply.addString ( m_message);
this->m_rpcPort.reply ( m_reply );
wrenchesVectorPtr->push_back(newWrench);
}
else yError() << "Failed to set new wrench values!";
else
{ this->m_message = this->m_message + " but " + command.get(0).asString() + " link found in the model" ;
this->m_reply.addString ( m_message );
this->m_rpcPort.reply ( m_reply );
}
}
else if (command.size() == 1 && command.get(0).isString()) {
std::string message = command.get(0).asString() + " wrench operation mode";
this->m_reply.addString (message);
this->m_rpcPort.reply ( m_reply );

this->m_mode = command.get(0).asString();

this->m_message = command.get(0).asString() + " wrench operation mode set";

// Delete the previous wrenches
if (wrenchesVectorPtr->size() != 0) {
this->m_message = this->m_message + " . Clearing previous wrenches.";
for (int i = 0; i < wrenchesVectorPtr->size(); i++)
{
boost::shared_ptr<ExternalWrench> wrench = wrenchesVectorPtr->at(i);
wrench->deleteWrench();
}
wrenchesVectorPtr->clear();
}

this->m_reply.addString (m_message);
this->m_rpcPort.reply ( m_reply );

}
else {
this->m_reply.clear();
this->m_reply.addString ( "ERROR: Incorrect command format" );
this->m_message = "ERROR: Incorrect command format. Insert [help] to know the correct command format";
this->m_reply.addString ( m_message );
this->m_rpcPort.reply ( this->m_reply );
}
}
Expand Down
12 changes: 2 additions & 10 deletions plugins/externalwrench/src/ExternalWrench.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,7 @@ bool ExternalWrench::getLink()
}

if(!link)
{
yError() << "externalWrench: " << wrenchPtr->link_name << " link is not found";
{
return false;
}

Expand Down Expand Up @@ -79,14 +78,7 @@ bool ExternalWrench::setWrench(physics::ModelPtr& _model,yarp::os::Bottle& cmd)
model = _model;

//get link name from command
std::string linkName = cmd.get(0).asString();
if (linkName.find_last_of("::") != 0) {
wrenchPtr->link_name = linkName.substr(linkName.find_last_of("::")+1); //case of scoped link name
}
else
{
wrenchPtr->link_name = linkName; //case of unscoped link name
}
wrenchPtr->link_name = cmd.get(0).asString();

if(getLink())
{
Expand Down

0 comments on commit f796908

Please sign in to comment.