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