void OptionsWindowSize::event(SDL_Event& e)
{
if (windowSizeButtons[1].isPressed(e) && !window->isFullscreen()) {
increaseResolution();
}
if (windowSizeButtons[0].isPressed(e) && !window->isFullscreen()) {
decreaseResolution();
}
}
void OptionsWindowSize::onGamepadButton(int key, int state, int elementID)
{
if (elementID == 2)
{
colour = { 255, 0, 0 };
if (key == SDL_CONTROLLER_BUTTON_DPAD_LEFT && state == GamepadButtonPressed && !window->isFullscreen())
{
decreaseResolution();
}
if (key == SDL_CONTROLLER_BUTTON_DPAD_RIGHT && state == GamepadButtonPressed && !window->isFullscreen())
{
increaseResolution();
}
}
else
{
colour = { 255, 255, 255 };
}
}
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());
}
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;
}
