Planner.hpp

Go to the documentation of this file.

#pragma once
#include <base/samples/RigidBodyState.hpp>
#include <sbpl_spline_primitives/SbplSplineMotionPrimitives.hpp>
#include "EnvironmentXYZTheta.hpp"
#include <trajectory_follower/SubTrajectory.hpp>
#include "PlannerConfig.hpp"
 
#include <memory>
 
class ARAPlanner;
 
namespace ugv_nav4d
{
 
class PlannerDump;
    
class Planner
{
protected:
    friend class PlannerDump;
    std::shared_ptr<EnvironmentXYZTheta> env;
    std::shared_ptr<ARAPlanner> planner;
    
    const sbpl_spline_primitives::SplinePrimitivesConfig splinePrimitiveConfig; 
    const Mobility mobility;
    traversability_generator3d::TraversabilityConfig traversabilityConfig;
    PlannerConfig plannerConfig;
    std::vector<int> solutionIds;
        
    std::vector<Eigen::Vector3d> previousStartPositions;
    
public:
    enum PLANNING_RESULT {
        GOAL_INVALID,
        START_INVALID, 
        NO_SOLUTION, 
        NO_MAP,
        INTERNAL_ERROR,
        FOUND_SOLUTION,
        TIMEOUT, 
    };
    
    Planner(const sbpl_spline_primitives::SplinePrimitivesConfig &primitiveConfig, 
        const traversability_generator3d::TraversabilityConfig &traversabilityConfig,
        const Mobility& mobility, 
        const PlannerConfig& plannerConfig);
      
    void updateMap(const traversability_generator3d::TravMap3d &map)
    {
        std::shared_ptr<traversability_generator3d::TravMap3d> mapPtr = std::make_shared<traversability_generator3d::TravMap3d>(map);
 
        if(!env)
        {
            env.reset(new EnvironmentXYZTheta(mapPtr, traversabilityConfig, splinePrimitiveConfig, mobility));
            env->setCorridorWidth(plannerConfig.corridorWidth);
            env->setGoalOrientationMargin(plannerConfig.goalOrientationMargin);
            env->setGoalDistanceMargin(plannerConfig.goalDistanceMargin);
        }
        else
        {
            env->updateMap(mapPtr);
            env->setCorridorWidth(plannerConfig.corridorWidth);
            env->setGoalOrientationMargin(plannerConfig.goalOrientationMargin);
            env->setGoalDistanceMargin(plannerConfig.goalDistanceMargin);
        }
    }
    
    void enablePathStatistics(bool enable);
 
    bool isEnvironmentInitialized() const { return static_cast<bool>(env); }
 
    std::vector<Motion> getMotions() const;
    
    PLANNING_RESULT plan(const base::Time& maxTime, const base::samples::RigidBodyState& start_pose,
                         const base::samples::RigidBodyState& end_pose, std::vector<trajectory_follower::SubTrajectory>& resultTrajectory2D,
                         std::vector<trajectory_follower::SubTrajectory>& resultTrajectory3D, bool dumpOnError = false, bool dumpOnSuccess = false);
   
    void setTravConfig(const traversability_generator3d::TraversabilityConfig& config);
    
    void setPlannerConfig(const PlannerConfig& config);
    
    const std::shared_ptr<const traversability_generator3d::TravMap3d> getTraversabilityMap() const;
    std::shared_ptr<trajectory_follower::SubTrajectory> findTrajectoryOutOfObstacle(const Eigen::Vector3d& start, double theta,
            const Eigen::Affine3d& ground2Body, bool setZToZero);
 
    private:
    bool calculateGoal(Eigen::Vector3d& goal_translation, const double yaw);
    bool tryGoal(const Eigen::Vector3d& translation, const double yaw);
 
};
 
}