/**
* Similar to the annotator's `process` function but trimmed to just
* handle a specific controller.
*
* @param cas The current common analysis system.
* @param controller The controller name which is also equivalent to the
* mongo db collection's name.
* @return UIMA error type id - UIMA_ERR_NONE on success.
*/
uima::TyErrorId processController(
uima::CAS& cas,
const std::string& controller
) {
log->logMessage("ControllerAnnotator::processController() begins");
// Parse MongoDB data into an urdf model.
MongoUrdf* urdf = new MongoUrdf(host);
typedef std::vector< std::map<std::string, ModelState> > statesT;
statesT states = urdf->getControllerStates(database, controller);
delete urdf;
// Initialize required indices.
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::ANIterator jsIter = cas.getAnnotationIndex(JointStateType).iterator();
uima::AnnotationFS ci, js;
std::vector<std::string> names;
int stamp, begin, end;
for (statesT::iterator it = states.begin(); it != states.end(); it++) {
ModelState desired = it->find("desired")->second;
ModelState actual = it->find("actual")->second;
ModelState error = it->find("error")->second;
names = desired.getJointNames();
stamp = desired.time;
//begin = 0, end = 0;
/*if (
jsIter.isValid() && jsIter.peekPrevious().isValid() &&
stamp < (js = jsIter.get()).getIntValue(jsTimeFtr)
) {
jsIter.moveToNext();
begin = js.getBeginPosition();
end = js.getEndPosition();
}*/
ci = cas.createAnnotation(ControllerInput, stamp, stamp);
ci.setStringValue(ciTypeFtr, utils::toUS(controller));
ci.setIntValue(ciTimeFtr, stamp);
ci.setFSValue(ciJnsFtr, utils::toStringArrayFS(cas, names));
ci.setFSValue(ciDesFtr, toJtp(desired.jointStates));
ci.setFSValue(ciActFtr, toJtp(actual.jointStates));
ci.setFSValue(ciErrFtr, toJtp(error.jointStates));
index.addFS(ci);
}
log->logMessage("ControllerAnnotator::processController() ends");
return UIMA_ERR_NONE;
}
/**
* Data processing.
*
* @param cas The current common analysis system.
* @param resultSpecification The specification of expected results as given
* by the annotator descriptor.
* @return UIMA error type id - UIMA_ERR_NONE on success.
*/
uima::TyErrorId process(
uima::CAS& cas,
const uima::ResultSpecification& resultSpecification
) {
log->logMessage("MovementAnnotator::process() begins");
// Save cas for use in other member functions.
currentCas = &cas;
// Intialize indices and the index iterator.
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::ANIterator jsIter = cas.getAnnotationIndex(JointState).iterator();
// Initialize reused variables.
uima::AnnotationFS js = jsIter.get(), move;
uima::FeatureStructure jtp;
uima::StringArrayFS names;
std::size_t size = js.getStringArrayFSValue(jsNameFtr).size();
uima::ArrayFS moves = cas.createArrayFS(size);
std::list<uima::DoubleArrayFS> prevPositions;
std::vector<bool> movingStates(size, false);
std::vector<double> variances;
std::size_t id;
// Init log stream.
uima::LogStream& logstream = log->getLogStream(uima::LogStream::EnMessage);
// Loop through joint states.
while (jsIter.isValid()) {
js = jsIter.get();
jtp = js.getFSValue(jsJtpFtr);
id = js.getBeginPosition();
names = js.getStringArrayFSValue(jsNameFtr);
prevPositions.push_back(jtp.getDoubleArrayFSValue(jtpPosFtr));
// If the list is too long pop the first element.
if (prevPositions.size() > observedJointStates) {
prevPositions.pop_front();
}
// If the list is too short continue adding the next element.
if (prevPositions.size() < observedJointStates) {
continue;
}
// Calculate variances in every joint's positions to detect movements.
variances = calculateVariances(prevPositions);
// Iterate over the observed variances.
for (std::size_t i = 0; i < variances.size(); i++) {
move = static_cast<uima::AnnotationFS>(moves.get(i));
icu::UnicodeString name = names.get(i).getBuffer();
if (1 == move.getBeginPosition()) { // Initial iteration.
moves.set(i, moveAnnotation(name, id, id));
} else { // Consecutive iterations.
bool moving = (variances[i] >= minVariance);
if ( // Movement state:
(!movingStates[i] && moving) || // wasn't + is
(movingStates[i] && !moving) // was + isn't
) {
index.addFS(move);
moves.set(i, moveAnnotation(name, id, id));
} else if ( // Movement state:
(!movingStates[i] && !moving) || // wasn't + isn't
(movingStates[i] && moving) // was + is
) {
moves.set(i, moveAnnotation(name, move.getBeginPosition(), id));
}
}
}
jsIter.moveToNext();
}
// Flush log.
logstream.flush();
// Add all remaining movements to the index - it might just be that the
// joints didn't move at all and those are still their initial states.
for (std::size_t i = 0; i < moves.size(); i++) {
move = static_cast<uima::AnnotationFS>(moves.get(i));
index.addFS(move);
}
log->logMessage("MovementAnnotator::process() ends");
return UIMA_ERR_NONE;
}
/**
* Data processing.
*
* @param cas The current common analysis system.
* @param resultSpecification The specification of expected results as given
* by the annotator descriptor.
* @return UIMA error type id - UIMA_ERR_NONE on success.
*/
uima::TyErrorId process(
uima::CAS& cas,
const uima::ResultSpecification& resultSpecification
) {
log->logMessage("MovementDirectionAnnotator::process() begins");
// Save cas for use in other member functions.
currentCas = &cas;
// Intialize the cas index and the movement index iterator.
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::ANIndex jsIndex = cas.getAnnotationIndex(JointState);
uima::ANIterator moveIter = cas.getAnnotationIndex(Movement).iterator();
// Initialize reused variables.
uima::AnnotationFS move, js;
uima::UnicodeStringRef name;
std::vector<uima::AnnotationFS> jointStates;
double pos, posDiff, posPrev = 0, posStart = 0;
std::size_t begin = 0, end;
int direction;
std::vector<int> movementStates;
// Loop through movement annotations.
while (moveIter.isValid()) {
move = moveIter.get();
jointStates = utils::selectCovered(jsIndex, move);
direction = 0;
end = move.getEndPosition();
begin = move.getBeginPosition();
// Loop through joint states related to this movement.
for (std::size_t i = 0; i <= jointStates.size(); i++) {
if (i < jointStates.size()) {
js = jointStates[i];
end = js.getEndPosition();
name = move.getStringValue(mvNameFtr);
pos = getPosByName(js, name);
posStart = pos;
posDiff = pos - posPrev;
} else {
// Force direction change to annotate the very last movement dir.
posDiff = (posDiff >= 0) ? 1 : -1;
}
// Act acording to movement direction.
switch (direction) {
case POSITIVE:
if (0 > posDiff) { // Direction changed, now negative.
index.addFS(posMoveAnnotation(name, begin, end, posStart, pos));
direction = NEGATIVE;
posStart = pos;
begin = js.getBeginPosition();
}
break;
case NEGATIVE:
if (0 < posDiff) { // Direction changed, now positive.
index.addFS(negMoveAnnotation(name, begin, end, posStart, pos));
direction = POSITIVE;
posStart = pos;
begin = js.getBeginPosition();
}
break;
default: // Only entered in initial iteration.
direction = (0 <= posDiff) ? POSITIVE : NEGATIVE;
}
posPrev = pos;
}
moveIter.moveToNext();
}
log->logMessage("MovementDirectionAnnotator::process() ends");
return UIMA_ERR_NONE;
}
/**
* DistanceToLimit annotations.
*
* @return UIMA error id. UIMA_ERR_NONE on success.
*/
uima::TyErrorId annotateLimits(
uima::CAS& cas,
const urdf::Model& model
) {
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::ANIndex jsIndex = cas.getAnnotationIndex(JointState);
uima::ANIterator jsIter = jsIndex.iterator();
uima::AnnotationFS js, dst;
uima::FeatureStructure jtp;
uima::StringArrayFS jsNames;
uima::DoubleArrayFS jtpPositions, jtpVelocities, jtpEfforts;
boost::shared_ptr<const urdf::Joint> joint;
boost::shared_ptr<urdf::JointLimits> limits;
std::vector<std::string> dstNames;
std::vector<double> upperLimits, lowerLimits, velocities, efforts;
while (jsIter.isValid()) {
js = jsIter.get();
jtp = js.getFSValue(jsJtpFtr);
// Get feature structure value arrays.
jsNames = js.getStringArrayFSValue(jsNameFtr);
jtpPositions = jtp.getDoubleArrayFSValue(jtpPosFtr);
jtpVelocities = jtp.getDoubleArrayFSValue(jtpEffFtr);
jtpEfforts = jtp.getDoubleArrayFSValue(jtpVelFtr);
// Clear storage vectors.
dstNames.clear();
upperLimits.clear();
lowerLimits.clear();
velocities.clear();
efforts.clear();
for (std::size_t i = 0; i < jsNames.size(); i++) {
joint = model.getJoint(jsNames.get(i).asUTF8());
limits = joint->limits;
// Limits are only relevant for some joint types.
if (
joint->type == urdf::Joint::REVOLUTE ||
joint->type == urdf::Joint::PRISMATIC
) {
dstNames.push_back(joint->name);
upperLimits.push_back(limits->upper - jtpPositions.get(i));
lowerLimits.push_back(limits->lower - jtpPositions.get(i));
velocities.push_back(limits->velocity - jtpVelocities.get(i));
efforts.push_back(limits->effort - jtpEfforts.get(i));
}
}
dst = cas.createAnnotation(DistanceToLimit,
js.getBeginPosition(), js.getEndPosition());
dst.setFSValue(dstNameFtr, utils::toStringArrayFS(cas, dstNames));
dst.setFSValue(dstUppFtr, utils::toDoubleArrayFS(cas, upperLimits));
dst.setFSValue(dstLowFtr, utils::toDoubleArrayFS(cas, lowerLimits));
dst.setFSValue(dstVelFtr, utils::toDoubleArrayFS(cas, velocities));
dst.setFSValue(dstEffFtr, utils::toDoubleArrayFS(cas, efforts));
index.addFS(dst);
jsIter.moveToNext();
}
return UIMA_ERR_NONE;
}