Skip to content

Commit

Permalink
yarp_dev: implement yarp messages for IPosition and IPositionDirect n…
Browse files Browse the repository at this point in the history 
…ew messages
  • Loading branch information
Alberto Cardellino committed Jan 15, 2016
1 parent f63766c commit d4c3d5e
Show file tree
Hide file tree
Showing 2 changed files with 222 additions and 1 deletion.
94 changes: 93 additions & 1 deletion src/libYARP_dev/src/modules/ControlBoardWrapper/RPCMessagesParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2298,6 +2298,98 @@ bool RPCMessagesParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& r
}
break;

case VOCAB_POSITION_MOVE:
{
if (ControlBoardWrapper_p->verbose())
yDebug("getTargetPosition\n");

ok = rpc_IPosCtrl2->getTargetPosition(cmd.get(2).asInt(), &dtmp);

response.addDouble(dtmp);
rec=true;
}
break;

case VOCAB_POSITION_MOVE_GROUP:
{
int len = cmd.get(2).asInt();
Bottle& in = *(cmd.get(3).asList());
int *jointList = new int[len];
double *refs = new double[len];

for(int j=0; j<len; j++)
{
jointList[j] = in.get(j).asInt();
}
ok = rpc_IPosCtrl2->getTargetPositions(len, jointList, refs);

Bottle& b = response.addList();
for (int i = 0; i < len; i++)
b.addDouble(refs[i]);

delete[] jointList;
delete[] refs;
}
break;

case VOCAB_POSITION_MOVES:
{
double *refs = new double[controlledJoints];
ok = rpc_IPosCtrl2->getTargetPositions(refs);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(refs[i]);
delete[] refs;
}
break;

case VOCAB_POSITION_DIRECT:
{
if (ControlBoardWrapper_p->verbose())
yDebug("getRefPosition\n");

ok = rpc_IPosDirect->getRefPosition(cmd.get(2).asInt(), &dtmp);

response.addDouble(dtmp);
rec=true;
}
break;

case VOCAB_POSITION_DIRECT_GROUP:
{
int len = cmd.get(2).asInt();
Bottle& in = *(cmd.get(3).asList());
int *jointList = new int[len];
double *refs = new double[len];

for(int j=0; j<len; j++)
{
jointList[j] = in.get(j).asInt();
}
ok = rpc_IPosDirect->getRefPositions(len, jointList, refs);

Bottle& b = response.addList();
for (int i = 0; i < len; i++)
b.addDouble(refs[i]);

delete[] jointList;
delete[] refs;
}
break;

case VOCAB_POSITION_DIRECTS:
{
double *refs = new double[controlledJoints];
ok = rpc_IPosDirect->getRefPositions(refs);
Bottle& b = response.addList();
int i;
for (i = 0; i < controlledJoints; i++)
b.addDouble(refs[i]);
delete[] refs;
}
break;

case VOCAB_LIM:
{
ok = rpc_IPid->getErrorLimit(cmd.get(2).asInt(), &dtmp);
Expand Down Expand Up @@ -2333,7 +2425,7 @@ bool RPCMessagesParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& r
}
break;

case VOCAB_MOTION_DONE_GROUP:
case VOCAB_MOTION_DONE_GROUP:
{
bool x = false;
int len = cmd.get(2).asInt();
Expand Down
129 changes: 129 additions & 0 deletions src/libYARP_dev/src/modules/RemoteControlBoard/RemoteControlBoard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -739,6 +739,44 @@ class yarp::dev::RemoteControlBoard :
return false;
}

bool get2V1I1IA1DA(int v1, int v2, const int n_joints, const int *joints, double *retVals, yarp::os::ConstString functionName = "")
{
Bottle cmd, response;
if (!isLive()) return false;

cmd.addVocab(VOCAB_GET);
cmd.addVocab(v1);
cmd.addVocab(v2);
cmd.addInt(n_joints);

Bottle& l1 = cmd.addList();
for (int i = 0; i < n_joints; i++)
l1.addInt(joints[i]);

bool ok = rpc_p.write(cmd, response);

if (CHECK_FAIL(ok, response))
{
int i;
Bottle& list = *(response.get(0).asList());
YARP_ASSERT(list.size() >= n_joints)

if (list.size() != n_joints)
{
yError("%s length of response does not match: expected %d, received %d\n ", functionName.c_str(), n_joints , list.size() );
return false;
}
else
{
for (i = 0; i < n_joints; i++)
{
retVals[i] = (double) list.get(i).asDouble();
}
return true;
}
}
}

bool get1V1B(int v, bool &val) {
Bottle cmd, response;
cmd.addVocab(VOCAB_GET);
Expand Down Expand Up @@ -812,6 +850,38 @@ class yarp::dev::RemoteControlBoard :
return false;
}

/**
* Helper method to get an array of double from the remote peer.
* @param v1 is the name of the command
* @param val is the array of double
* @return true/false on success/failure
*/
bool get1V1DA(int v1, double *val) {
if (!isLive()) return false;
Bottle cmd, response;
cmd.addVocab(VOCAB_GET);
cmd.addVocab(v1);
bool ok = rpc_p.write(cmd, response);

if (CHECK_FAIL(ok, response)) {
int i;
Bottle& l = *(response.get(1).asList());
if (&l == 0)
return false;

int njs = l.size();
YARP_ASSERT (nj == njs);
for (i = 0; i < nj; i++)
val[i] = l.get(i).asDouble();

getTimeStamp(response, lastStamp);


return true;
}
return false;
}

/**
* Helper method to get an array of double from the remote peer.
* @param v1 is the name of the command
Expand Down Expand Up @@ -2307,6 +2377,47 @@ class yarp::dev::RemoteControlBoard :
return set1VDA(VOCAB_POSITION_MOVES, refs);
}

/** Get the last position reference for the specified axis.
* This is the dual of PositionMove and shall return only values sent using
* IPositionControl interface.
* If other interfaces like IPositionDirect are implemented by the device, this call
* must ignore their values, i.e. this call must never return a reference sent using
* IPositionDirect::SetPosition
* @param ref last reference sent using PositionMove functions
* @return true/false on success/failure
*/
virtual bool getTargetPosition(const int joint, double *ref)
{
return get1V1I1D(VOCAB_POSITION_MOVE, joint, ref);
}

/** Get the last position reference for all axes.
* This is the dual of PositionMove and shall return only values sent using
* IPositionControl interface.
* If other interfaces like IPositionDirect are implemented by the device, this call
* must ignore their values, i.e. this call must never return a reference sent using
* IPositionDirect::SetPosition
* @param ref last reference sent using PositionMove functions
* @return true/false on success/failure
*/
virtual bool getTargetPositions(double *refs)
{
return get1V1DA(VOCAB_POSITION_MOVES, refs);
}

/** Get the last position reference for the specified group of axes.
* This is the dual of PositionMove and shall return only values sent using
* IPositionControl interface.
* If other interfaces like IPositionDirect are implemented by the device, this call
* must ignore their values, i.e. this call must never return a reference sent using
* IPositionDirect::SetPosition
* @param ref last reference sent using PositionMove functions
* @return true/false on success/failure
*/
virtual bool getTargetPositions(const int n_joint, const int *joints, double *refs)
{
return get1V1I1IA1DA(VOCAB_POSITION_MOVE_GROUP, n_joint, joints, refs);
}

/**
* Set relative position. The command is relative to the
Expand Down Expand Up @@ -3344,6 +3455,9 @@ class yarp::dev::RemoteControlBoard :
return CHECK_FAIL(ok, response);
}

//
// IPositionDirect Interface
//
bool setPositionDirectMode()
{
return set1V(VOCAB_POSITION_DIRECT);
Expand Down Expand Up @@ -3390,6 +3504,21 @@ class yarp::dev::RemoteControlBoard :
return true;
}

bool getRefPosition(const int joint, double* ref)
{
return get1V1I1D(VOCAB_POSITION_DIRECT, joint, ref);
}

bool getRefPositions(double* refs)
{
return get1V1DA(VOCAB_POSITION_DIRECTS, refs);
}

bool getRefPositions(const int n_joint, const int* joints, double* refs)
{
return get1V1I1IA1DA(VOCAB_POSITION_DIRECT_GROUP, n_joint, joints, refs);
}

//
// IVelocityControl2 Interface
//
Expand Down

0 comments on commit d4c3d5e

Please sign in to comment.