PhotonVision / photonvision

PhotonVision is the free, fast, and easy-to-use computer vision solution for the FIRST Robotics Competition.
https://photonvision.org
GNU General Public License v3.0
264 stars 180 forks source link

Setting up pose estimation and simulation objects causes crashes #1222

Open mjhealy opened 6 months ago

mjhealy commented 6 months ago

Describe the bug I've implemented what I believe to be valid (albeit simple) sample vision subsystem in C++ for the team I coach. This uses a single camera, running under the simulator. Code is available here.

I've reduced the code reproducing the issue to simply setting up the PhotonPoseEstimator in a Vision subsystem class, derived from frc2::SubsystemBase, with all code in all methods of the class having been commented out, leaving only the behaviors exercised by the objects declared in the subsystem (see below):

  // In Vision.h
  // Note: camera name, robot-to-camera transform, and simulated camera
  // properties are all hard-coded.  We might want to make them more like
  // the camera we're really using on "production" code, but for this
  // sample, that's less of an issue.
  const std::string m_cameraName{"vision-sim"};
  const frc::Transform3d kRobotToCam{
      frc::Translation3d(/*x=*/0.5_m, /*y=*/0.0_m, /*z=*/0.5_m),
      frc::Rotation3d(/*roll=*/0_deg, /*pitch=*/-30_deg, /*yaw=*/0_deg)};
  const photon::SimCameraProperties m_simCameraProperties =
      photon::SimCameraProperties::PI4_LIFECAM_640_480();
  const bool ENABLE_WIREFRAME_RENDERING_ON_RAW_VIDEO = true;

  const frc::AprilTagFieldLayout m_fieldLayout =
      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo);

  photon::PhotonPoseEstimator m_photonEstimator{
      m_fieldLayout, photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
      // Note: camera must be passed into pose estimator as an r-value reference
      // due to the constructor signature for the class.  (Unclear why it
      // doesn't take a shared_ptr to the camera, allowing sharing with the
      // subsystem.)
      std::move(photon::PhotonCamera{m_cameraName}), kRobotToCam};

  // Pose estimator holds the camera; get back a shared pointer to let us
  // set up the PhotonCameraSim.
  std::shared_ptr<photon::PhotonCamera> m_camera =
      m_photonEstimator.GetCamera();

  // Handles camera simulation.
  // ENABLING THIS CAUSES A CRASH, EVEN IF WE DO NOTHING WITH IT.
  // HOWEVER, ALLOCATING THE SUBSYSTEM WITH "new" AND LEAKING IT
  // SEEMS TO DUCK THIS???
  photon::PhotonCameraSim m_cameraSim{m_camera.get(), m_simCameraProperties};

  // Handles overall vision system simulation.
  photon::VisionSystemSim m_visionSim{m_cameraName};

We're using a "command-based" robot, generated using the templates in the WPILib tools, with the RobotContainer class containing an instance of the Vision subsystem.

Expected behavior:

Actual behavior:

To Reproduce Steps to reproduce the behavior:

  1. Download the sample code here.
  2. In PreprocessorConfig.h, change #define LEAK_VISION_TO_WORK_AROUND_CLEANUP_BUG to #undef LEAK_VISION_TO_WORK_AROUND_CLEANUP_BUG.
  3. Run under simulator.
  4. Observe crash on launch.

Please note that this behavior is being seen on a Mac running Sonoma (MacOS 14.3 (23D56)). I do not have ready access to a Windows box, and thus have not yet been able to test it there.

Screenshots / Videos

Platform:

Additional context None at this time.

mcm001 commented 6 months ago

Cloned and on windows at least with and without LEAK_VISION_TO_WORK_AROUND_CLEANUP_BUG defined I do not see crashes? whack

mcm001 commented 6 months ago

WSL2 ubuntu 22.04, same result with and without LEAK_VISION_TO_WORK_AROUND_CLEANUP_BUG defined/undefined. I don't own a mac so you might need to help troubleshoot this further, but to be honest I'm not super sure where to start.

mcm001 commented 6 months ago

Can you get any sort of backtrace or exception that's getting thrown in the crash from above?