trajectory_planner.h

#pragma once
 
#include <optional>
 
#include "software/ai/navigator/obstacle/obstacle.hpp"
#include "software/ai/navigator/trajectory/trajectory_path.h"
#include "software/ai/navigator/trajectory/trajectory_path_with_cost.h"
 
class TrajectoryPlanner
{
   public:
    TrajectoryPlanner();
 
    std::optional<TrajectoryPath> findTrajectory(
        const Point &start, const Point &destination, const Vector &initial_velocity,
        const KinematicConstraints &constraints,
        const std::vector<ObstaclePtr> &obstacles, const Rectangle &navigable_area,
        const std::optional<Point> &prev_sub_destination = std::nullopt);
 
   private:
    double calculateCost(const TrajectoryPathWithCost &traj_with_cost) const;
 
    TrajectoryPathWithCost getDirectTrajectoryWithCost(
        const Point &start, const Point &destination, const Vector &initial_velocity,
        const KinematicConstraints &constraints,
        const std::vector<ObstaclePtr> &obstacles);
 
    TrajectoryPathWithCost getTrajectoryWithCost(
        const TrajectoryPath &trajectory, const std::vector<ObstaclePtr> &obstacles,
        const std::optional<TrajectoryPathWithCost> &sub_traj_with_cost,
        const std::optional<double> sub_traj_duration_s);
 
    double getFirstNonCollisionTime(const TrajectoryPath &traj_path,
                                    const std::vector<ObstaclePtr> &obstacles,
                                    const double search_end_time_s) const;
 
    std::pair<double, ObstaclePtr> getFirstCollisionTime(
        const TrajectoryPath &traj_path, const std::vector<ObstaclePtr> &obstacles,
        const double start_time_s, const double search_end_time_s) const;
 
    double getLastNonCollisionTime(const TrajectoryPath &traj_path,
                                   const std::vector<ObstaclePtr> &obstacles,
                                   const double search_end_time_s) const;
 
    std::vector<Point> getSubDestinations(const Point &start, const Point &destination,
                                          const Rectangle &navigable_area) const;
 
    static std::vector<Vector> getRelativeSubDestinations();
 
    const std::vector<Vector> relative_sub_destinations;
    static constexpr std::array<double, 4> SUB_DESTINATION_DISTANCES_METERS = {0.4, 1.1,
                                                                               2.3, 3};
    static constexpr unsigned int NUM_SUB_DESTINATION_ANGLES                = 16;
    static constexpr Angle MIN_SUB_DESTINATION_ANGLE = Angle::fromDegrees(20);
    static constexpr Angle MAX_SUB_DESTINATION_ANGLE = Angle::fromDegrees(140);
 
    const double SUB_DESTINATION_STEP_INTERVAL_SEC         = 0.2;
    const double COLLISION_CHECK_STEP_INTERVAL_SEC         = 0.1;
    const double FORWARD_COLLISION_CHECK_STEP_INTERVAL_SEC = 0.05;
    const double MAX_FUTURE_COLLISION_CHECK_SEC            = 2.0;
 
    const double SUB_DESTINATION_CLOSE_BONUS_THRESHOLD_METERS = 0.1;
    const double SUB_DESTINATION_CLOSE_BONUS_COST             = -0.3;
};