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);
}
/**
* 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;
}
void CASDeserializer::deserializeData(uima::internal::SerializedCAS const & crSerializedCAS,
uima::CAS & rCAS) {
uima::CAS * baseCAS = rCAS.getBaseCas();
baseCAS->reset();
deserializeHeapsAndStringTable( crSerializedCAS, *baseCAS );
deserializeIndexedFSs(CONST_CAST(vector<SerializedCAS::TyNum>&,crSerializedCAS.iv_vecIndexedFSs), *baseCAS);
// deserializeDocument(crSerializedCAS, rCAS); // do this last because only here is the document annotation created
}
/**
* 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;
}
/**
* 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;
}
/**
* TaskSpacePosition annotations.
*
* @return UIMA error id. UIMA_ERR_NONE on success.
*/
uima::TyErrorId annotateTaskSpace(
uima::CAS& cas,
const urdf::Model& model
) {
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::FeatureStructure ts;
boost::shared_ptr<urdf::Link> link;
std::vector< boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
for (std::size_t i = 0, size = links.size(); i < size; i++) {
link = links[i];
ts = cas.createFS(TaskSpacePosition);
ts.setFSValue(tsXyzFtr,
utils::toDoubleArrayFS(cas, MongoUrdf::getPosition(link)));
ts.setFSValue(tsRpyFtr,
utils::toDoubleArrayFS(cas, MongoUrdf::getRotation(link)));
index.addFS(ts);
}
return UIMA_ERR_NONE;
}
/**
* Create a Joint Trajectory Point Feature Structure from a vector of
* JointState objects.
*
* @param jointStates Source vector of JointState objects.
* @return Joint Trajectory Point with all the information from the objects.
*/
uima::FeatureStructure toJtp(const std::vector<JointState>& jointStates) {
std::size_t size = jointStates.size(), i = 0;
std::vector<JointState>::const_iterator it;
uima::DoubleArrayFS positions = currentCas->createDoubleArrayFS(size);
uima::DoubleArrayFS effort = currentCas->createDoubleArrayFS(size);
uima::DoubleArrayFS velocities = currentCas->createDoubleArrayFS(size);
uima::DoubleArrayFS accelerations = currentCas->createDoubleArrayFS(size);
for (it = jointStates.begin(); it != jointStates.end(); it++, i++) {
positions.set(i, it->position);
effort.set(i, it->effort);
velocities.set(i, it->velocity);
accelerations.set(i, it->acceleration);
}
uima::FeatureStructure jtp = currentCas->createFS(JointTrajectoryPoint);
jtp.setFSValue(jtpPosFtr, positions);
jtp.setFSValue(jtpEffFtr, effort);
jtp.setFSValue(jtpVelFtr, velocities);
jtp.setFSValue(jtpAccFtr, accelerations);
return jtp;
}
inline void fromHeapCellTempl( lowlevel::TyHeapCell tyCell, uima::CAS & cas, short & rResult) {
rResult = cas.getHeap()->getShort(tyCell);
}
inline void fromHeapCellTempl( lowlevel::TyHeapCell tyCell, uima::CAS & cas, char & rResult) {
rResult = cas.getHeap()->getByte(tyCell);
}
//new types
inline void fromHeapCellTempl( lowlevel::TyHeapCell tyCell, uima::CAS & cas,bool & rResult) {
rResult = cas.getHeap()->getBoolean(tyCell);
}
inline void fromHeapCellTempl( lowlevel::TyHeapCell tyCell, uima::CAS & cas, UnicodeStringRef & rResult) {
rResult = cas.getHeap()->getFSAsString((lowlevel::TyFS)tyCell);
}
inline void fromHeapCellTempl( lowlevel::TyHeapCell tyCell, uima::CAS & cas, FeatureStructure & rResult) {
// if tyCell is an annotation, might create FS from different CAS
CAS & other = cas.getCasForTyFS(tyCell);
rResult = uima::internal::FSPromoter::promoteFS((lowlevel::TyFS)tyCell, other);
}
void CASDeserializer::deserializeIndexedFSs(vector<SerializedCAS::TyNum> & crIndexFSs,
uima::CAS & rCAS) {
uima::internal::CASImpl & rCASImpl = uima::internal::CASImpl::promoteCAS(rCAS);
uima::lowlevel::FSHeap & crHeap = rCASImpl.getHeap();
uima::lowlevel::IndexRepository * crIndexRep = &rCASImpl.getIndexRepository();
uima::lowlevel::FSHeap::TyFSHeap const & rTempFSHeap = crHeap.iv_clTemporaryHeap;
SerializedCAS::TyNum iMaxOffset = rTempFSHeap.getTopOfHeap();
vector<SerializedCAS::TyNum>::const_iterator cit, loopit;
vector<SerializedCAS::TyNum> perLoopIndexedFSs;
cit = crIndexFSs.begin();
int numViews = *cit++;
int loopSize = *cit;
crIndexRep->reset();
// deserialize base CAS
if (loopSize > 0) {
lastSegmentUsed = 0;
perLoopIndexedFSs.insert(perLoopIndexedFSs.end(), cit+1, cit+1+loopSize);
cit += loopSize + 1;
for (loopit = perLoopIndexedFSs.begin(); loopit != perLoopIndexedFSs.end(); ++loopit) {
assert( *loopit < iMaxOffset );
crIndexRep->add( *loopit );
}
}
// book keeping for all Sofas
rCAS.getBaseCas()->iv_sofaCount = 1; // reserve for initial view
FSIndex fsIdx = crIndexRep->getIndex(CAS::INDEXID_SOFA);
FSIterator fsIt = fsIdx.iterator();
while (fsIt.isValid()) {
SofaFS aSofa = (SofaFS) fsIt.get();
if ( 0 == aSofa.getSofaID().compare(UnicodeString(CAS::NAME_DEFAULT_SOFA)) ) {
rCAS.registerInitialSofa();
} else {
// only bump sofa count if not initial View
rCAS.bumpSofaCount();
}
rCAS.getView(aSofa)->registerView(aSofa);
fsIt.moveToNext();
}
for (int view = 1; view <= numViews; view++) {
// Check if sofa's index has anything in it
loopSize = *cit;
if (0 == loopSize) {
cit++;
continue;
}
CAS* tcas = rCAS.getViewBySofaNum(view);
uima::internal::CASImpl & crTCASImpl = uima::internal::CASImpl::promoteCAS(*tcas);
crIndexRep = &crTCASImpl.getIndexRepository();
crIndexRep->reset();
perLoopIndexedFSs.clear();
perLoopIndexedFSs.insert(perLoopIndexedFSs.end(), cit+1, cit+1+loopSize);
cit += loopSize + 1;
for (loopit = perLoopIndexedFSs.begin(); loopit != perLoopIndexedFSs.end(); ++loopit) {
assert( *loopit < iMaxOffset );
crIndexRep->add( *loopit );
}
tcas->pickupDocumentAnnotation();
}
}
/**
* 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;
}
inline void fromHeapCellTempl( lowlevel::TyHeapCell tyCell, uima::CAS & cas, INT64 & rResult) {
rResult = cas.getHeap()->getLong(tyCell);
}
/**
* 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;
}
// for blob deserialization
void CASDeserializer::deserializeBlob(void * buffer, uima::CAS & rCAS) {
vector<SerializedCAS::TyNum> iv_vecIndexedFSs;
// check blob "key" and version
int* intPtr = (int*) buffer;
// Check if blob needs byteswap
#if defined(WORDS_BIGENDIAN)
char key[] = "UIMA";
char yek[] = "AMIU";
#else
char key[] = "AMIU";
char yek[] = "UIMA";
#endif
if (intPtr[0] == ((int*)yek)[0]) {
swapBlob(buffer);
}
CHECK(intPtr[0] == ((int*)key)[0]);
CHECK(intPtr[1] == 1);
// get a heap of references
uima::internal::CASImpl & crCASImpl = uima::internal::CASImpl::promoteCAS(rCAS);
uima::lowlevel::FSHeap & crHeap = crCASImpl.getHeap();
uima::lowlevel::FSHeap::TyFSHeap & tyTempHeap = crHeap.iv_clTemporaryHeap;
uima::lowlevel::FSHeap::TyStringHeap & tyStringHeap = crHeap.iv_clTemporaryStringHeap;
uima::lowlevel::FSHeap::TyStringRefHeap & tyStringRefHeap = crHeap.iv_clTemporaryStringRefHeap;
uima::lowlevel::FSHeap::Ty8BitHeap & ty8BitHeap = crHeap.iv_clTemporary8BitHeap;
uima::lowlevel::FSHeap::Ty16BitHeap & ty16BitHeap = crHeap.iv_clTemporary16BitHeap;
uima::lowlevel::FSHeap::Ty64BitHeap & ty64BitHeap = crHeap.iv_clTemporary64BitHeap;
// deserialize FSHeap
tyTempHeap.reset();
size_t uiFSHeapLength = intPtr[2];
if (uiFSHeapLength > 1) {
CHECK(intPtr[3] == 0);
tyTempHeap.increaseHeap(uiFSHeapLength-1);
memcpy(tyTempHeap.getHeapStart(), intPtr+3, 4*uiFSHeapLength);
}
intPtr += 3 + uiFSHeapLength;
// deserialize StringTable
tyStringHeap.reset();
size_t uiStringHeapLength = intPtr[0];
size_t uialignedStrLen = 2 * ((uiStringHeapLength + 1)/2);
if (uiStringHeapLength > 1) {
CHECK(((short*)intPtr)[2] == 0); // check the first short after the length
tyStringHeap.increaseHeap(uiStringHeapLength-1);
memcpy(tyStringHeap.getHeapStart(), intPtr+1, 2*uiStringHeapLength);
}
intPtr += 1 + uialignedStrLen/2;
// deserialize StringRef
tyStringRefHeap.reset();
size_t uiRefHeapLength = intPtr[0];
if (uiRefHeapLength > 1) {
CHECK(intPtr[1] == 0);
tyStringRefHeap.increaseHeap(uiRefHeapLength-1);
memcpy(tyStringRefHeap.getHeapStart(), intPtr+1, 4*uiRefHeapLength);
}
intPtr += 1 + uiRefHeapLength;
// create FS indexes
size_t uiIndexedFSLength = intPtr[0];
if (uiIndexedFSLength > 0) {
uima::CAS * baseCAS = rCAS.getBaseCas();
iv_vecIndexedFSs.resize(uiIndexedFSLength);
memcpy(&iv_vecIndexedFSs[0], intPtr+1, 4*uiIndexedFSLength);
deserializeIndexedFSs(iv_vecIndexedFSs, *baseCAS);
}
intPtr += 1 + uiIndexedFSLength;
//8bit heap
ty8BitHeap.reset();
size_t ui8BitHeapLength = intPtr[0];
size_t uialigned8BitHeapLen = 4 * ((ui8BitHeapLength + 3)/4);
if (ui8BitHeapLength > 1) {
//CHECK(((char*)intPtr)[1] == 0);
ty8BitHeap.increaseHeap(ui8BitHeapLength-1);
memcpy(ty8BitHeap.getHeapStart(), intPtr+1, ui8BitHeapLength);
}
intPtr += 1 + uialigned8BitHeapLen/4;
//16 bit heap
ty16BitHeap.reset();
size_t ui16BitHeapLength = intPtr[0];
size_t uialigned16BitHeapLen = 2 * ((ui16BitHeapLength + 1)/2);
if (ui16BitHeapLength > 1) {
//CHECK(((short*)intPtr)[2] == 0);
ty16BitHeap.increaseHeap(ui16BitHeapLength-1);
memcpy(ty16BitHeap.getHeapStart(), intPtr+1, 2*ui16BitHeapLength);
}
intPtr += 1 + uialigned16BitHeapLen/2;
//64 bit heap
ty64BitHeap.reset();
size_t ui64BitHeapLength = intPtr[0];
if (ui64BitHeapLength > 0) {
//CHECK(intPtr[1] == 0);
ty64BitHeap.increaseHeap(ui64BitHeapLength-1);
memcpy(ty64BitHeap.getHeapStart(), intPtr+1, 8*ui64BitHeapLength);
}
}
inline void fromHeapCellTempl( lowlevel::TyHeapCell tyCell, uima::CAS & cas, double & rResult) {
rResult = cas.getHeap()->getDouble(tyCell);
}
/**
* 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;
}
