/**
   * 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;
  }
예제 #2
0
bool parseModelState(ModelState &ms, TiXmlElement* config)
{
  ms.clear();

  const char *name_char = config->Attribute("name");
  if (!name_char)
  {
    CONSOLE_BRIDGE_logError("No name given for the model_state.");
    return false;
  }
  ms.name = std::string(name_char);

  const char *time_stamp_char = config->Attribute("time_stamp");
  if (time_stamp_char)
  {
    try {
      double sec = boost::lexical_cast<double>(time_stamp_char);
      ms.time_stamp.set(sec);
    }
    catch (boost::bad_lexical_cast &e) {
      CONSOLE_BRIDGE_logError("Parsing time stamp [%s] failed: %s", time_stamp_char, e.what());
      return false;
    }
  }

  TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state");
  if (joint_state_elem)
  {
    JointStateSharedPtr joint_state;
    joint_state.reset(new JointState());

    const char *joint_char = joint_state_elem->Attribute("joint");
    if (joint_char)
      joint_state->joint = std::string(joint_char);
    else
    {
      CONSOLE_BRIDGE_logError("No joint name given for the model_state.");
      return false;
    }
    
    // parse position
    const char *position_char = joint_state_elem->Attribute("position");
    if (position_char)
    {

      std::vector<std::string> pieces;
      boost::split( pieces, position_char, boost::is_any_of(" "));
      for (unsigned int i = 0; i < pieces.size(); ++i){
        if (pieces[i] != ""){
          try {
            joint_state->position.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
          }
          catch (boost::bad_lexical_cast &/*e*/) {
            throw ParseError("position element ("+ pieces[i] +") is not a valid float");
          }
        }
      }
    }

    // parse velocity
    const char *velocity_char = joint_state_elem->Attribute("velocity");
    if (velocity_char)
    {

      std::vector<std::string> pieces;
      boost::split( pieces, velocity_char, boost::is_any_of(" "));
      for (unsigned int i = 0; i < pieces.size(); ++i){
        if (pieces[i] != ""){
          try {
            joint_state->velocity.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
          }
          catch (boost::bad_lexical_cast &/*e*/) {
            throw ParseError("velocity element ("+ pieces[i] +") is not a valid float");
          }
        }
      }
    }

    // parse effort
    const char *effort_char = joint_state_elem->Attribute("effort");
    if (effort_char)
    {

      std::vector<std::string> pieces;
      boost::split( pieces, effort_char, boost::is_any_of(" "));
      for (unsigned int i = 0; i < pieces.size(); ++i){
        if (pieces[i] != ""){
          try {
            joint_state->effort.push_back(boost::lexical_cast<double>(pieces[i].c_str()));
          }
          catch (boost::bad_lexical_cast &/*e*/) {
            throw ParseError("effort element ("+ pieces[i] +") is not a valid float");
          }
        }
      }
    }

    // add to vector
    ms.joint_states.push_back(joint_state);
  }
  return false;
}
예제 #3
0
bool parseModelState(ModelState &ms, TiXmlElement* config)
{
  ms.clear();

  const char *name_char = config->Attribute("name");
  if (!name_char)
  {
    logError("No name given for the model_state.");
    return false;
  }
  ms.name = std::string(name_char);

  const char *time_stamp_char = config->Attribute("time_stamp");
  if (time_stamp_char)
  {
    double newDouble;
    if( stringToDouble(time_stamp_char,newDouble) )
    {
      double sec = newDouble;
      ms.time_stamp.set(sec);
    }
    else {
      logError("Parsing time stamp [%s] failed.", time_stamp_char);
      return false;
    }
  }

  TiXmlElement *joint_state_elem = config->FirstChildElement("joint_state");
  if (joint_state_elem)
  {
    boost::shared_ptr<JointState> joint_state;
    joint_state.reset(new JointState());

    const char *joint_char = joint_state_elem->Attribute("joint");
    if (joint_char)
      joint_state->joint = std::string(joint_char);
    else
    {
      logError("No joint name given for the model_state.");
      return false;
    }

    // parse position
    const char *position_char = joint_state_elem->Attribute("position");
    if (position_char)
    {

      std::vector<std::string> pieces;
      splitString( position_char, pieces);
      for (unsigned int i = 0; i < pieces.size(); ++i){
        double newDouble;
        if (pieces[i] != ""){
          if( stringToDouble(pieces[i],newDouble) )
          {
            joint_state->position.push_back(newDouble);
          } else {
            throw ParseError("position element ("+ pieces[i] +") is not a valid float");
          }
        }
      }
    }

    // parse velocity
    const char *velocity_char = joint_state_elem->Attribute("velocity");
    if (velocity_char)
    {

      std::vector<std::string> pieces;
      splitString( position_char, pieces);

      for (unsigned int i = 0; i < pieces.size(); ++i){

        if (pieces[i] != ""){
          double newDouble;
          if( stringToDouble(time_stamp_char,newDouble) )
          {
            joint_state->velocity.push_back(newDouble);
          } else {
            throw ParseError("velocity element ("+ pieces[i] +") is not a valid float");
          }
        }
      }
    }

    // parse effort
    const char *effort_char = joint_state_elem->Attribute("effort");
    if (effort_char)
    {

      std::vector<std::string> pieces;
      splitString( position_char, pieces);

      for (unsigned int i = 0; i < pieces.size(); ++i){
        if (pieces[i] != ""){
          double newDouble;
          if( stringToDouble(time_stamp_char,newDouble) )
          {
            joint_state->effort.push_back(newDouble);
          } else {
            throw ParseError("effort element ("+ pieces[i] +") is not a valid float");
          }
        }
      }
    }

    // add to vector
    ms.joint_states.push_back(joint_state);
  }
};