int main(int argc, char** argv) {
ros::init(argc, argv, "waypoint_publisher");
ros::NodeHandle nh;
ROS_INFO("Started waypoint_publisher.");
ros::V_string args;
ros::removeROSArgs(argc, argv, args);
if (args.size() != 2) {
ROS_ERROR(
"Usage: waypoint_publisher <waypoint_file> (one per line, space separated: wait_time [s] x[m] y[m] z[m] yaw[deg])");
return -1;
}
std::vector<WaypointWithTime> waypoints;
const float DEG_2_RAD = M_PI / 180.0;
std::ifstream wp_file(args.at(1).c_str());
if (wp_file.is_open()) {
double t, x, y, z, yaw;
// Only read complete waypoints.
while (wp_file >> t >> x >> y >> z >> yaw) {
waypoints.push_back(WaypointWithTime(t, x, y, z, yaw * DEG_2_RAD));
}
wp_file.close();
ROS_INFO("Read %d waypoints.", (int )waypoints.size());
}
void getWaypoints(std::vector<WaypointWithTime> & waypoints) {
const float DEG_2_RAD = M_PI / 180.0;
std::ifstream wp_file(inputFilePath.c_str());
if (wp_file.is_open()) {
double t, x, y, z, yaw;
// Only read complete waypoints.
while (wp_file >> t >> x >> y >> z >> yaw) {
waypoints.push_back(WaypointWithTime(t, x, y, z, yaw * DEG_2_RAD));
}
wp_file.close();
std::vector<WaypointWithTime> newWaypoints;
// increaseResolution(waypoints, newWaypoints, 1);
increaseResolution(waypoints, newWaypoints, 0.3, 0.3, 0.1);
waypoints = newWaypoints;
ROS_INFO("Read %d waypoints.", (int )waypoints.size());
}
