vision_subsystem.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <frc/filter/LinearFilter.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include <frc2/command/SubsystemBase.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/length.h>
#include <wpi/DataLog.h>
 
#include <stop_token>
#include <string>
#include <thread>
 
#include "argos_lib/config/config_types.h"
#include "argos_lib/general/debouncer.h"
#include "argos_lib/general/interpolation.h"
#include "argos_lib/general/nt_subscriber.h"
#include "argos_lib/general/odometry_aim.h"
#include "constants/interpolation_maps.h"
#include "networktables/NetworkTable.h"
#include "networktables/NetworkTableEntry.h"
#include "networktables/NetworkTableInstance.h"
#include "networktables/NetworkTableValue.h"
#include "swerve_drive_subsystem.h"
 
enum class whichCamera { LEFT_CAMERA = 0, RIGHT_CAMERA, INVALID };
 
struct unitlessChassisSpeeds {
  double forwardSpeed{0.0};
  double leftSpeed{0.0};
  double ccwSpeed{0.0};
};
 
class LimelightTarget {
 private:
  frc::Pose3d m_robotPose;              
  frc::Pose3d m_robotPoseWPI;           
  frc::Pose3d m_targetPoseCamSpace;     
  frc::Pose3d m_targetPoseRobotSpace;   
  bool m_hasTargets;                    
  units::degree_t m_pitch;              
  units::degree_t m_yaw;                
  double m_area;                        
  units::millisecond_t m_totalLatency;  
  frc::LinearFilter<units::degree_t> m_txFilter;
  frc::LinearFilter<units::degree_t> m_tyFilter;
  frc::LinearFilter<units::meter_t> m_zFilter;
  bool m_resetFilterFlag;
  double m_tid;  
  int m_hb;      
 
 public:
  LimelightTarget()
      : m_txFilter{frc::LinearFilter<units::degree_t>::SinglePoleIIR(0.01, 0.02_s)}
      , m_tyFilter{frc::LinearFilter<units::degree_t>::SinglePoleIIR(0.01, 0.02_s)}
      , m_zFilter{frc::LinearFilter<units::meter_t>::SinglePoleIIR(0.01, 0.02_s)}
      , m_resetFilterFlag{false} {}
 
  struct tValues {
    frc::Pose3d robotPose;              
    frc::Pose3d robotPoseWPI;           
    frc::Pose3d tagPoseCamSpace;        
    frc::Pose3d tagPoseRobotSpace;      
    bool hasTargets;                    
    units::degree_t m_pitch;            
    units::degree_t m_yaw;              
    double m_area;                      
    double tagID;                       
    units::millisecond_t totalLatency;  
    int hb;                             
  };
 
  tValues GetTarget(bool filter, std::string cameraName);
 
  bool HasTarget();
 
  void ResetFilters(std::string cameraName);
 
  void ResetOnNextTarget();
};
 
class CameraInterface {
 public:
  CameraInterface();
 
  LimelightTarget m_target;  
 
  std::optional<LimelightTarget> GetClosestTarget();
 
  void SetDriverMode(bool mode);
 
  void RequestTargetFilterReset();
};
 
struct ActiveVisionTarget {
  whichCamera cameraID{whichCamera::INVALID};
  std::chrono::time_point<std::chrono::steady_clock> sampleTime{};
  LimelightTarget::tValues target{};
};
 
class VisionSubsystem : public frc2::SubsystemBase {
 public:
  VisionSubsystem(const argos_lib::RobotInstance instance, SwerveDriveSubsystem* pDriveSubsystem);
 
  enum class InterpolationMode { LinearInterpolation, Polynomial, Trig };
 
  [[nodiscard]] LimelightTarget::tValues GetLeftCameraTargetValues();
 
  [[nodiscard]] LimelightTarget::tValues GetRightCameraTargetValues();
 
  void SetPipeline(uint16_t tag);
 
  void RequestFilterReset();
 
  void SetAimWhileMove(bool val);
  [[nodiscard]] bool IsAimWhileMoveActive();
 
  void SetOdometryAiming(bool val);
  [[nodiscard]] bool IsOdometryAimingActive();
 
  void SetEnableStaticRotation(bool val);
  [[nodiscard]] bool IsStaticRotationEnabled();
 
  void Periodic() override;
 
  void Disable();
 
  std::optional<LimelightTarget::tValues> GetSeeingCamera(bool resample = false);
  std::optional<ActiveVisionTarget> getWhichCamera(bool resample = true);
  std::optional<whichCamera> getLatestReefSide();
  std::optional<frc::Pose2d> GetClosestReefTagPoseInCamSpace(bool resample = false);
  std::optional<frc::Translation2d> GetFieldCentricReefAlignmentError(bool resample = false);
  std::optional<frc::Translation2d> GetRobotSpaceReefAlignmentError(bool resample = false);
  std::optional<units::degree_t> GetOrientationCorrection(bool resample = false);
  void SetLeftAlign(bool val);
  void SetRightAlign(bool val);
  void SetAlgaeAlign(bool val);
  [[nodiscard]] bool LeftAlignmentRequested();
  [[nodiscard]] bool RightAlignmentRequested();
  [[nodiscard]] bool AlgaeAlignmentRequested();
  void SetL1Active(bool val);
  [[nodiscard]] bool isL1Active();
  void SetAlgaeModeActive(bool val);
  [[nodiscard]] bool isAlgaeModeActive();
 
  [[nodiscard]] bool robotAligned(bool resample = true);
 
  [[nodiscard]] std::optional<unitlessChassisSpeeds> getVisionAlignmentSpeeds(double scalingFactor = 1.0);
 
 private:
  const std::string leftCameraTableName = "/limelight-left";
  const std::string rightCameraTableName = "/limelight-right";
 
  // Components (e.g. motor controllers and sensors) should generally be
  // declared private and exposed only through public methods.
  CameraInterface m_cameraInterface;                     
  std::shared_ptr<nt::NetworkTable> m_leftCameraTable;   
  std::shared_ptr<nt::NetworkTable> m_rightCameraTable;  
 
  argos_lib::RobotInstance
      m_instance;  
  SwerveDriveSubsystem* m_pDriveSubsystem;     
  LimelightTarget::tValues m_oldTargetValues;  
  bool m_usePolynomial;                        
  bool m_useTrigonometry;                      
  bool m_isAimWhileMoveActive;                 
  bool m_enableStaticRotation;                 
  bool m_isOdometryAimingActive;               
  bool m_isLeftAlignActive;                    
  bool m_isRightAlignActive;                   
  bool m_isAlgaeAlignActive;                   
  bool m_isL1Active;                           
  bool m_isAlgaeModeActive;                    
  bool m_isRotationGoodEnough;                 
  int m_latestLeftHeartbeat;
  int m_latestRightHeartbeat;
  ActiveVisionTarget m_activeVisionTarget;
  argos_lib::Debouncer m_leftFresh;
  argos_lib::Debouncer m_rightFresh;
  std::optional<whichCamera> m_latestReefSide;                 
  std::chrono::steady_clock::time_point m_latestReefSpotTime;  
  argos_lib::NTSubscriber m_leftCameraFrameUpdateSubscriber;   
  argos_lib::NTSubscriber m_rightCameraFrameUpdateSubscriber;  
  std::jthread m_yawUpdateThread;
 
  wpi::log::StructLogEntry<frc::Pose2d> m_leftCameraMegaTag2PoseLogger;
  wpi::log::StructLogEntry<frc::Pose2d> m_rightCameraMegaTag2PoseLogger;
 
  void UpdateYaw(std::stop_token stopToken);
};