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