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