SentenceAnnotation::SentenceAnnotation(uima::CAS& aCas, size_t begin, size_t end, const TokenAnnotation& firstToken, const TokenAnnotation& lastToken, const ParagraphAnnotation& paragraph)
{
FSIndexRepository& indexRep = aCas.getIndexRepository();
annotation = aCas.createAnnotation(tSentenceAnnotation, begin, end);
setFirstToken(firstToken);
setLastToken(lastToken);
setParagraph(paragraph);
indexRep.addFS(annotation);
}
/**
* Create a movement annotation feature structure given a name, a begin and an
* end position.
*
* @param name Unicode string giving the feature structure's name feature
* value.
* @param from Unsigned integer giving the annotation begin position.
* @param to Unsigned integer giving the annotation end position.
* @return Movement annotation feature structure.
*/
uima::AnnotationFS moveAnnotation(
const icu::UnicodeString& name,
const std::size_t& from,
const std::size_t& to
) {
uima::AnnotationFS move = currentCas->createAnnotation(Movement, from, to);
move.setStringValue(mvNameFtr, name);
return move;
}
/**
* 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;
}
/**
* Create an annoptation of type NegativeMovement with timely and positional
* begin and end positions.
*
* @param begin Value for the annotation begin position - time.
* @param end Value for the annotation end position - time.
* @param posStart Value for the startPosition feature - position.
* @param posEnd Value for the endPosition feature - position.
* @return Resulting NegativeMovement annotation feature structure.
*/
uima::AnnotationFS negMoveAnnotation(
const uima::UnicodeStringRef& name,
const std::size_t& begin,
const std::size_t& end,
const double& posStart,
const double& posEnd
) {
uima::AnnotationFS fs;
fs = currentCas->createAnnotation(NegativeMovement, begin, end);
fs.setStringValue(pJnFtr, name);
fs.setDoubleValue(nSpFtr, posStart);
fs.setDoubleValue(nEpFtr, posEnd);
return fs;
}
/**
* 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;
}