/** * 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; }