Camera rework

camera-config/HandCameraCalibration.yml

@@ -18,13 +18,14 @@ intrinsic_params:
       rows: 3
       cols: 3
       dt: d
-      data: [ 6.4936764773277400e+02, 0., 3.3546152305967058e+02, 0.,
-          6.4776804982533793e+02, 2.4785660889077488e+02, 0., 0., 1. ]
+      data: [ 6.4936764773277400e+02, 0., 3.3546152305967058e+02,
+              0., 6.4776804982533793e+02, 2.4785660889077488e+02,
+              0., 0., 1. ]
    distortion_coefficients: !!opencv-matrix
       rows: 5
       cols: 1
       dt: d
-      data: [ -4.5670452858419575e-01, 2.1512832208218641e-01,
+      data: [ -4.5670452858419575e-01,2.1512832208218641e-01,
           -1.1179480533495888e-03, -5.7450430930239835e-04, 0. ]
 extrinsic_params: !!opencv-matrix
   rows: 4

camera-config/HandCameraConfig.yml

@@ -0,0 +1,11 @@
+%YAML:1.0
+---
+name: "hand"
+description: "Camera located on the hand of the rover."
+# Make sure that this camera_id matches the one defined in the udev rules.
+camera_id: 20
+
+format: "jpeg"
+width: 640
+height: 480
+framerate: 30

rover-config/50-rover-cameras.rules

@@ -13,4 +13,4 @@ SUBSYSTEM=="video4linux", ATTRS{idVendor}=="0c45", ATTRS{idProduct}=="6369", ATT
 SUBSYSTEM=="video4linux", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="9230", ATTR{index}=="0", SYMLINK+="video30"
 
 # Mast camera
-SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", ATTR{index}=="0", SYMLINK+="video40"
+SUBSYSTEM=="video4linux", ATTRS{idVendor}=="05a3", ATTRS{idProduct}=="9230", ATTR{index}=="0", SYMLINK+="video40"

src/CMakeLists.txt

@@ -67,6 +67,7 @@ message("========================================")
 # here to avoid the compiler spamming warnings at us for stuff in the Eigen headers.
 find_package(Eigen3 REQUIRED NO_MODULE)
 include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR})
+include_directories("/usr/local/include")
 
 # Find the WebSocket++ library and Boost (provides the ASIO backend for Websocket++ and also
 # provides the program_options argument parser). Only the `system` component of Boost is
@@ -132,8 +133,6 @@ endif()
 
 add_library(camera SHARED
   camera/Camera.cpp
-  camera/CameraParams.cpp
-  camera/CameraConfig.cpp
 )
 target_link_libraries(camera PUBLIC ${OpenCV_LIBS})
 
@@ -199,11 +198,11 @@ endif()
 # hardware-agnostic utilities and common code for world interface
 add_library(world_interface_core STATIC
   world_interface/gps_common_interface.cpp
-  ar/Detector.cpp
-  ar/MarkerSet.cpp
-  ar/MarkerPattern.cpp
-  ar/Tag.cpp
-  ar/read_landmarks.cpp
+  #ar/Detector.cpp
+  #ar/MarkerSet.cpp
+  #ar/MarkerPattern.cpp
+  #ar/Tag.cpp
+  #ar/read_landmarks.cpp
   world_interface/motor/base_motor.cpp
   )
 target_include_directories(world_interface_core SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
@@ -298,21 +297,21 @@ target_link_libraries(Rover
   ${vision_libs}
 )
 
-add_executable(TunePID TunePID.cpp)
-target_link_libraries(TunePID
-  ${rover_libs}
-  can_interface
-)
-target_link_libraries(TunePID ${vision_libs})
+# add_executable(TunePID TunePID.cpp)
+# target_link_libraries(TunePID
+#   ${rover_libs}
+#   can_interface
+# )
+# target_link_libraries(TunePID ${vision_libs})
 
 if(WITH_TESTS)
   add_executable(tests
     Tests.cpp
     # AR Detection tests
-    ar/DetectorTests.cpp
-    ar/MarkerSetTests.cpp
+    # ar/DetectorTests.cpp
+    # ar/MarkerSetTests.cpp
     # Camera tests
-    ../tests/camera/CameraParamsTests.cpp
+    # ../tests/camera/CameraParamsTests.cpp
     # GPS tests
     ../tests/gps/GPSDatumTest.cpp
     ../tests/gps/GPSConverterTest.cpp
@@ -352,9 +351,9 @@ if(WITH_TESTS)
   catch_discover_tests(tests)
 endif()
 
-add_executable(gpsd_test
-  gps/gpsd_test.cpp)
-target_link_libraries(gpsd_test gps)
+# add_executable(gpsd_test
+#   gps/gpsd_test.cpp)
+# target_link_libraries(gpsd_test gps)
 
 if (WORLD_INTERFACE STREQUAL "REAL")
   add_executable(LimitSwitchCalibration

src/Constants.cpp

@@ -47,15 +47,19 @@ const double MAX_WHEEL_VEL = WHEEL_RADIUS * MAX_DRIVE_PWM / PWM_PER_RAD_PER_SEC;
 const double MAX_DTHETA = kinematics::DiffDriveKinematics(EFF_WHEEL_BASE)
 							  .wheelVelToRobotVel(-MAX_WHEEL_VEL, MAX_WHEEL_VEL)(2);
 
-// TODO: We need to recalibrate the camera, since we replaced it with a different one.
-const char* MAST_CAMERA_CONFIG_PATH = "../camera-config/MastCameraCalibration.yml";
-const robot::types::CameraID MAST_CAMERA_ID = "mast";
-
-const char* FOREARM_CAMERA_CONFIG_PATH = "../camera-config/WristCameraCalibration.yml";
-const robot::types::CameraID FOREARM_CAMERA_ID = "wrist";
-
-const char* HAND_CAMERA_CONFIG_PATH = "../camera-config/HandCameraCalibration.yml";
-const robot::types::CameraID HAND_CAMERA_ID = "hand";
+const robot::types::CameraID MAST_CAMERA_ID = 40;
+const robot::types::CameraID WRIST_CAMERA_ID = 30;
+const robot::types::CameraID HAND_CAMERA_ID = 20;
+const robot::types::CameraID TEST_CAMERA_ID = 0;
+
+constexpr frozen::unordered_map<robot::types::CameraID, frozen::string, 4>
+	CAMERA_CONFIG_PATHS = {{MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"},
+						   {WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"},
+						   {HAND_CAMERA_ID, "../camera-config/HandCameraConfig.yml"},
+                           {TEST_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"}};
+
+constexpr frozen::unordered_map<frozen::string, robot::types::CameraID, 3> CAMERA_NAME_TO_ID =
+	{{"mast", MAST_CAMERA_ID}, {"wrist", WRIST_CAMERA_ID}, {"hand", HAND_CAMERA_ID}};
 
 /**
    @deprecated No need for this constant once we fully switch over the Mission Control PlanViz

src/Constants.h

@@ -53,15 +53,15 @@ extern const double MAX_DTHETA;
 
 // TODO: We need to recalibrate the camera, since we replaced it with a different one.
 // TODO: rename cameras (in MC as well) as appropriate
-extern const char* MAST_CAMERA_CONFIG_PATH;
 extern const robot::types::CameraID MAST_CAMERA_ID;
-
-extern const char* FOREARM_CAMERA_CONFIG_PATH;
-extern const robot::types::CameraID FOREARM_CAMERA_ID;
-
-extern const char* HAND_CAMERA_CONFIG_PATH;
+extern const robot::types::CameraID WRIST_CAMERA_ID;
 extern const robot::types::CameraID HAND_CAMERA_ID;
 
+extern const frozen::unordered_map<robot::types::CameraID, frozen::string, 4>
+	CAMERA_CONFIG_PATHS;
+extern const frozen::unordered_map<frozen::string, robot::types::CameraID, 3>
+	CAMERA_NAME_TO_ID;
+
 extern const uint16_t WS_SERVER_PORT;
 
 /**