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);
}
};
bool parseModelState(ModelState &ms, TiXmlElement* config)
{
ms.clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
printf("No name given for the model_state. \n");
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) {
printf("Parsing time stamp [%s] failed: %s \n", time_stamp_char, e.what());
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
{
printf("No joint name given for the model_state. \n");
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);
}
};
