move_primitive.h

#pragma once
 
 
#include "proto/primitive/primitive_types.h"
#include "software/ai/hl/stp/tactic/primitive.h"
#include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h"
#include "software/ai/navigator/trajectory/bang_bang_trajectory_2d.h"
#include "software/ai/navigator/trajectory/trajectory_planner.h"
#include "software/world/world.h"
 
class MovePrimitive : public Primitive
{
   public:
    MovePrimitive(const Robot &robot, const Point &destination, const Angle &final_angle,
                  const TbotsProto::MaxAllowedSpeedMode &max_allowed_speed_mode,
                  const TbotsProto::ObstacleAvoidanceMode &obstacle_avoidance_mode,
                  const TbotsProto::DribblerMode &dribbler_mode,
                  const TbotsProto::BallCollisionType &ball_collision_type,
                  const AutoChipOrKick &auto_chip_or_kick,
                  std::optional<double> cost_override = std::nullopt);
 
    ~MovePrimitive() override = default;
 
    std::pair<std::optional<TrajectoryPath>, std::unique_ptr<TbotsProto::Primitive>>
    generatePrimitiveProtoMessage(
        const World &world,
        const std::set<TbotsProto::MotionConstraint> &motion_constraints,
        const std::map<RobotId, TrajectoryPath> &robot_trajectories,
        const RobotNavigationObstacleFactory &obstacle_factory) override;
 
    void getVisualizationProtos(
        TbotsProto::ObstacleList &obstacle_list_out,
        TbotsProto::PathVisualization &path_visualization_out) const override;
 
   private:
    void updateObstacles(const World &world,
                         const std::set<TbotsProto::MotionConstraint> &motion_constraints,
                         const std::map<RobotId, TrajectoryPath> &robot_trajectories,
                         const RobotNavigationObstacleFactory &obstacle_factory);
 
    Robot robot;
    Point destination;
    Angle final_angle;
 
    TbotsProto::DribblerMode dribbler_mode;
    AutoChipOrKick auto_chip_or_kick;
 
    TbotsProto::BallCollisionType ball_collision_type;
    TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode;
    TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode;
 
    // List of all obstacles that the robot should avoid
    std::vector<ObstaclePtr> obstacles;
    // List of only the motion constraint obstacles that the robot should avoid
    std::vector<ObstaclePtr> field_obstacles;
 
    BangBangTrajectory2D trajectory;
    std::optional<TrajectoryPath> traj_path;
    BangBangTrajectory1DAngular angular_trajectory;
    TrajectoryPlanner planner;
 
    constexpr static unsigned int NUM_TRAJECTORY_VISUALIZATION_POINTS = 10;
};