static std::vector<WP> readWaypoint(const char *filename)
{
std::ifstream ifs(filename);
std::string line;
std::getline(ifs, line); // Remove first line
size_t ncol = countColumn(line);
std::vector<WP> waypoints;
if (ncol == 3)
{
while (std::getline(ifs, line))
{
waypoints.push_back(parseWaypoint(line, false));
}
size_t last = waypoints.size() - 1;
for (size_t i = 0; i < waypoints.size(); ++i)
{
double yaw;
if (i == last)
{
yaw = atan2(waypoints[i-1].pose.position.y - waypoints[i].pose.position.y,
waypoints[i-1].pose.position.x - waypoints[i].pose.position.x);
yaw -= M_PI;
}
else
{
yaw = atan2(waypoints[i+1].pose.position.y - waypoints[i].pose.position.y,
waypoints[i+1].pose.position.x - waypoints[i].pose.position.x);
}
waypoints[i].pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
}
}
else if (ncol == 4)
{
while (std::getline(ifs, line))
{
waypoints.push_back(parseWaypoint(line, true));
}
}
return waypoints;
}
void WaypointLoaderNode::loadWaypoints(const char *filename, std::vector<autoware_msgs::waypoint> *wps)
{
std::ifstream ifs(filename);
if (!ifs)
return;
std::string line;
std::getline(ifs, line); // get first line
std::vector<std::string> contents;
parseColumns(line, &contents);
std::getline(ifs, line); // remove second line
while (std::getline(ifs, line))
{
autoware_msgs::waypoint wp;
parseWaypoint(line, contents, &wp);
wps->push_back(wp);
}
planningVelocity(&*wps);
}
void ConfigManager::parseFile(QString fname){
tok.init();
tok.seterrorhandler(&tokerrorhandler);
tok.settokens(tokens);
tok.setcommentlinesequence("#");
// read the entire file!
QFile file(fname);
if(!file.open(QIODevice::ReadOnly))
throw Exception().set("could not open config file");
QByteArray b = file.readAll();
if(b.isNull() || b.isEmpty())
throw Exception().set("could not read config file");
b.append((char)0);
file.close();
const char *data = b.constData();
tok.reset(data);
bool done = false;
while(!done){
// at the top level we parse frames and
// variables
int t = tok.getnext();
switch(t){
case T_VAR:
parseVars();
break;
case T_WINDOW:
parseWindow();
break;
case T_END:
done=true;
break;
case T_PORT:
port = tok.getnextint();
break;
case T_SENDPORT:
udpSendPort = tok.getnextint();
break;
// the UDP client now sets its address from the first packet received
// but this will override it
case T_SENDADDR:
tok.getnextstring(udpSendAddr);
UDPClient::getInstance()->setAddress(udpSendAddr);
break;
case T_VALIDTIME:
DataManager::dataValidInterval = tok.getnextfloat();
break;
case T_SENDINTERVAL:
sendInterval = tok.getnextfloat();
break;
case T_UPDATEINTERVAL:
graphicalUpdateInterval = tok.getnextfloat()*1000;
break;
case T_AUDIO:
parseAudio();
break;
case T_WAYPOINT:
parseWaypoint();
break;
default:
throw UnexpException(&tok,"'var', 'frame', config data or end of file");
}
}
}