plan_constraint.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

#include <ompl/base/Constraint.h>
#include <ompl/base/ConstrainedSpaceInformation.h>
#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>

#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <ompl/geometric/planners/est/BiEST.h>

#include <thread>
#include <chrono>

#include <se2ez/core.h>
#include <se2ez/plan.h>
#include <se2ez/gui.h>
#include <se2ez/gui/planpanel.h>

using namespace se2ez;

int main(int argc, char **argv)
{
    if (argc < 5)
    {
        std::cout << "Usage: ./plan_constraint [filename] [start] [goal] [constraint]" << std::endl;
        return -1;
    }

    auto robot = io::loadRobot(std::string(argv[1]));
    if (!robot)
        return -1;

    auto constraints = plan::loadConstraints(robot, std::string(argv[1]));
    auto constraint = constraints.find(std::string(argv[4]));
    if (constraint == constraints.end())
    {
        SE2EZ_ERROR("Constraint %1% does not exist in %2%!", argv[4], argv[1]);
        return -1;
    }

    plan::EZPlansConstraint planner(robot, constraint->second, plan::PROJECTION);
    planner.initialize();

    planner.setPlanner("prm:PRM");
    planner.setStartGoal(std::string(argv[2]), std::string(argv[3]));

    ompl::base::PlannerStatus solved = planner.setup->solve(10.0);

    if (solved)
    {
        std::cout << "Found solution!" << std::endl;
        auto &path = planner.setup->getSolutionPath();
        path.interpolate();
    }
    else
    {
        std::cout << "No solution found" << std::endl;
        return 0;
    }

    QApplication a(argc, argv);
    gui::MainWindow gui;

    auto panel = new gui::PlanPanel(robot, "Robot1");
    gui.addPanel(panel);
    gui.show();

    panel->setPath(planner.extractPath());

    return a.exec();
}