MotionBenchmakerDemo.h

#ifndef MOTION_BENCHMAKER_DEMO_H_
#define MOTION_BENCHMAKER_DEMO_H_
 
#include <string>
#include <vector>
#include <map>
#include <memory>
#include <chrono>
 
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/Planner.h>
#include <ompl/geometric/SimpleSetup.h>
#include <boost/property_tree/ptree.hpp>
#include <vamp/collision/environment.hh>

struct PlanningResult
{
    bool solved = false;
    double planningTime = 0.0;
    double simplificationTime = 0.0;
    unsigned int planningIterations = 0;
    unsigned int pathVertices = 0;
    double pathCost = 0.0;
};

class MotionBenchmakerDemo
{
public:
    MotionBenchmakerDemo(const std::string &robotName, const std::string &problemFile,
                         const std::string &plannerName = "RRTConnect");

    std::vector<PlanningResult> benchmarkProblem(const std::string &problemName, unsigned int numTrials = 1,
                                                 double timeoutSeconds = 5.0);

    std::map<std::string, std::vector<PlanningResult>> benchmarkAll(unsigned int numTrials = 1,
                                                                    double timeoutSeconds = 5.0,
                                                                    bool print_failures = false);

    const std::vector<std::string> &getProblemNames() const
    {
        return problemNames_;
    }

    static void printStatistics(const std::string &problemName, const std::vector<PlanningResult> &results);
 
private:
    std::string robotName_;
    std::string plannerName_;
    std::vector<std::string> problemNames_;
    std::map<std::string, std::vector<boost::property_tree::ptree>> problems_;
    std::shared_ptr<ompl::base::StateSpace> space_;
    std::shared_ptr<ompl::geometric::SimpleSetup> ss_;
 
    // Environment for collision checking
    using Environment = vamp::collision::Environment<vamp::FloatVector<vamp::FloatVectorWidth>>;
    std::shared_ptr<Environment> currentEnv_;

    void loadProblemsFromJSON(const std::string &filename);

    void initializeStateSpace();

    bool setupProblem(const std::string &problemName, const boost::property_tree::ptree &problemData);

    PlanningResult solveInstance(const std::string &problemName, const boost::property_tree::ptree &problemData,
                                 double timeoutSeconds);
    PlanningResult benchmarkInstance(const std::string &problemName, const boost::property_tree::ptree &problemData,
                                     unsigned int benchmarkTrials, double timeoutSeconds);

    std::shared_ptr<ompl::base::Planner> createPlanner(const std::shared_ptr<ompl::base::SpaceInformation> &si) const;
};
 
#endif