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()); }
WaypointWithTime getPointBetween(const WaypointWithTime & a, const WaypointWithTime & b, int step, int totalSteps, int minTime) { double t = minTime; double x = between(a.position[0], b.position[0], step, totalSteps); double y = between(a.position[1], b.position[1], step, totalSteps); double z = between(a.position[2], b.position[2], step, totalSteps); double yaw = between(a.yaw, b.yaw, step, totalSteps); return WaypointWithTime(t, x, y, z, yaw); }
void increaseResolution(std::vector<WaypointWithTime> & waypoints, std::vector<WaypointWithTime> & newWaypoints, int factor) { newWaypoints.push_back(waypoints[0]); for (int i=1; i < waypoints.size(); ++i) { for (int j=1; j <= factor; ++j) { double t = waypoints[i].waiting_time; double x = waypoints[i-1].position[0] + ((waypoints[i].position[0] - waypoints[i-1].position[0]) * j / factor); double y = waypoints[i-1].position[1] + ((waypoints[i].position[1] - waypoints[i-1].position[1]) * j / factor); double z = waypoints[i-1].position[2] + ((waypoints[i].position[2] - waypoints[i-1].position[2]) * j / factor); double yaw = waypoints[i-1].yaw + ((waypoints[i].yaw - waypoints[i-1].yaw) * j / factor); newWaypoints.push_back(WaypointWithTime(t, x, y, z, yaw)); } } }
bool GlobalPlanner::getGlobalPath() { std::vector<Cell> path; if (!FindPath(path)) { return false; } waypoints.resize(0); // Use actual position instead of the center of the cell waypoints.push_back(WaypointWithTime(0, currPos.x, currPos.y, currPos.z, yaw)); double lastYaw = yaw; path.push_back(path[path.size()-1]); for (int i=1; i < path.size()-1; ++i) { Cell p = path[i]; double newYaw = angle(p, path[i+1], lastYaw); waypoints.push_back(WaypointWithTime(0, p.x()+0.5, p.y()+0.5, p.z()+0.5, newYaw)); lastYaw = newYaw; } waypoints.push_back(WaypointWithTime(0, path[path.size()-1].x()+0.5, path[path.size()-1].y()+0.5, 2, 0)); increaseResolution(0.3, 0.05, 0.1 * kNanoSecondsInSecond); return true; }
void increaseResolution(std::vector<WaypointWithTime> & waypoints, std::vector<WaypointWithTime> & newWaypoints, double minDist, double minRot, double minTime) { newWaypoints.push_back(waypoints[0]); for (int i=1; i < waypoints.size(); ++i) { double diffX = std::abs(waypoints[i].position[0] - waypoints[i-1].position[0]); double diffY = std::abs(waypoints[i].position[1] - waypoints[i-1].position[1]); double diffZ = std::abs(waypoints[i].position[2] - waypoints[i-1].position[2]); double diffYaw = std::abs(waypoints[i].yaw - waypoints[i-1].yaw); int factor = (int) std::max( std::max(0.0, diffX / minDist), std::max(diffY / minDist, diffZ / minDist)); factor = std::max(factor, (int) (diffYaw / minRot)) + 1; for (int j=1; j <= factor; ++j) { double t = std::max(minTime, (waypoints[i-1].waiting_time + waypoints[i].waiting_time) / (2*factor)); double x = between(waypoints[i-1].position[0], waypoints[i].position[0], j, factor); double y = between(waypoints[i-1].position[1], waypoints[i].position[1], j, factor); double z = between(waypoints[i-1].position[2], waypoints[i].position[2], j, factor); double yaw = between(waypoints[i-1].yaw, waypoints[i].yaw, j, factor); // double yaw = waypoints[i-1].yaw; newWaypoints.push_back(WaypointWithTime(t, x, y, z, yaw)); } } }
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()); }