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"); } } }