Fix Simulator Interface + Optimize CMake (#371)

* Fix Simulator Interface + Optimize CMake

* Minor formatting, added comments, CAN CMake update

* Formatting

* Implement openCamera

Author: imisaacwu

src/CAN/CMakeLists.txt

@@ -1,4 +1,6 @@
 cmake_minimum_required(VERSION 3.8)
 
+if(WORLD_INTERFACE STREQUAL "REAL")
 	add_executable(FakeCANBoard FakeCANBoard.cpp)
-		target_link_libraries(FakeCANBoard can_interface real_world_interface)
+	target_link_libraries(FakeCANBoard can_interface real_world_interface)
+endif()

src/CMakeLists.txt

@@ -85,18 +85,19 @@ link_libraries(frozen::frozen)
 # Find argparse, library used for parsing command line arguments
 find_package(argparse REQUIRED)
 
-
 # Find the JSON library
 find_package(nlohmann_json 3.2.0 REQUIRED)
 
-# Find the CAN library; contains packet and protocol definitions and does not
-# actually require physical CAN to be present.
-FetchContent_Declare(
+if(WORLD_INTERFACE STREQUAL "REAL")
+  # Find the CAN library; contains packet and protocol definitions and does not
+  # actually require physical CAN to be present.
+  FetchContent_Declare(
     HindsightCAN
     GIT_REPOSITORY https://github.com/huskyroboticsteam/HindsightCAN.git
     GIT_TAG ce04c1a2dced3eb0e5beaaf39b2fcc436655e790
-)
-FetchContent_MakeAvailable(HindsightCAN)
+  )
+  FetchContent_MakeAvailable(HindsightCAN)
+endif()
 
 FetchContent_Declare(LoguruGitRepo
 	GIT_REPOSITORY "https://github.com/emilk/loguru"
@@ -154,30 +155,32 @@ add_library(utils SHARED
   world_interface/data.cpp)
 target_link_libraries(utils ${OpenCV_LIBS})
 
-## ====== CAN Interfaces =======
-# Stub CAN interface is used for the tests (and for the Rover if
-# CAN is disabled) and the real CAN interface is used for the Rover if CAN is
-# enabled.
-
-# Common CAN source files
-
-# **DON'T MAKE THIS SHARED**
-# Making this library shared causes some memory fuckery
-# No clue why but CAN I/O goes to shit. Don't do it.
-add_library(can_interface STATIC)
-target_sources(can_interface PRIVATE
-  CAN/CANMotor.cpp
-  CAN/CANUtils.cpp
-)
+if(WORLD_INTERFACE STREQUAL "REAL")
+  ## ====== CAN Interfaces =======
+  # Stub CAN interface is used for the tests (and for the Rover if
+  # CAN is disabled) and the real CAN interface is used for the Rover if CAN is
+  # enabled.
+
+  # Common CAN source files
+
+  # **DON'T MAKE THIS SHARED**
+  # Making this library shared causes some memory fuckery
+  # No clue why but CAN I/O goes to shit. Don't do it.
+  add_library(can_interface STATIC)
+  target_sources(can_interface PRIVATE
+    CAN/CANMotor.cpp
+    CAN/CANUtils.cpp
+  )
 
-# Hardware specific source files
-target_sources(can_interface PRIVATE
-  CAN/CAN.cpp)
-target_link_libraries(can_interface PUBLIC
-  HindsightCAN
-  Threads::Threads
-  utils
-)
+  # Hardware specific source files
+  target_sources(can_interface PRIVATE
+    CAN/CAN.cpp)
+  target_link_libraries(can_interface PUBLIC
+    HindsightCAN
+    Threads::Threads
+    utils
+  )
+endif()
 
 ## GPS Implementations
 
@@ -208,31 +211,27 @@ add_library(world_interface_core STATIC
 target_include_directories(world_interface_core SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
 target_link_libraries(world_interface_core PUBLIC ${vision_libs} opencv_aruco ${OpenCV_LIBS} utils camera)
 
-# CAN library (above) requires some utilities from this
-target_link_libraries(can_interface PUBLIC utils)
-
-add_library(stub_world_interface STATIC
-  world_interface/noop_world.cpp)
-
-add_library(real_world_interface STATIC
-  world_interface/real_world_interface.cpp
-  world_interface/motor/can_motor.cpp
-)
-
-add_library(simulator_interface STATIC
-  world_interface/simulator_interface.cpp
-  world_interface/motor/sim_motor.cpp
-)
-
-target_link_libraries(real_world_interface gps_interface can_interface world_interface_core)
-target_link_libraries(simulator_interface world_interface_core)
-target_link_libraries(stub_world_interface world_interface_core)
-
 if(WORLD_INTERFACE STREQUAL "REAL")
+  # CAN library (above) requires some utilities from this
+  target_link_libraries(can_interface PUBLIC utils)
+
+  add_library(real_world_interface STATIC
+    world_interface/real_world_interface.cpp
+    world_interface/motor/can_motor.cpp
+  )
+  target_link_libraries(real_world_interface gps_interface can_interface world_interface_core)
   add_library(world_interface ALIAS real_world_interface)
 elseif(WORLD_INTERFACE STREQUAL "SIMULATOR")
+  add_library(simulator_interface STATIC
+    world_interface/simulator_interface.cpp
+    world_interface/motor/sim_motor.cpp
+  )
+  target_link_libraries(simulator_interface world_interface_core)
   add_library(world_interface ALIAS simulator_interface)
 else()
+  add_library(stub_world_interface STATIC
+    world_interface/noop_world.cpp)
+  target_link_libraries(stub_world_interface world_interface_core)
   add_library(world_interface ALIAS stub_world_interface)
 endif()
 
@@ -273,7 +272,7 @@ add_executable(Rover Rover.cpp)
 add_library(rover_common STATIC
   Globals.cpp
   control_interface.cpp
-  )
+)
 target_link_libraries(rover_common
   utils
   websocket_utils
@@ -296,13 +295,6 @@ 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})
-
 if(WITH_TESTS)
   add_executable(tests
     Tests.cpp
@@ -359,6 +351,13 @@ if (WORLD_INTERFACE STREQUAL "REAL")
   add_executable(LimitSwitchCalibration
     LimitCalib.cpp)
   target_link_libraries(LimitSwitchCalibration real_world_interface)
+
+  add_executable(TunePID TunePID.cpp)
+  target_link_libraries(TunePID
+    ${rover_libs}
+    can_interface
+  )
+  target_link_libraries(TunePID ${vision_libs})
 endif()
 
 add_compile_options(

src/Constants.cpp

@@ -58,15 +58,15 @@ const robot::types::CameraID DRILL_CAMERA_ID = 540;
 const robot::types::CameraID MICROSCOPE_ID = 500;
 
 const std::unordered_map<robot::types::CameraID, std::string> CAMERA_CONFIG_PATHS = {
-   {HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"},
-   {WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"},
-   {MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"},
-   {RAND_CAMERA_ID, "../camera-config/RandCameraCalibration.yml"},
-   {CJ1_CAMERA_ID, "../camera-config/CJ1.yml"},
-   {BOX_CAMERA_ID, "../camera-config/BoxCamera.yml"},
-   {CJ3_CAMERA_ID, "../camera-config/CJ3.yml"},
-   {DRILL_CAMERA_ID, "../camera-config/DrillCamera.yml"},
-   {MICROSCOPE_ID, "../camera-config/Microscope.yml"},
+	{HAND_CAMERA_ID, "../camera-config/HandCameraCalibration.yml"},
+	{WRIST_CAMERA_ID, "../camera-config/WristCameraCalibration.yml"},
+	{MAST_CAMERA_ID, "../camera-config/MastCameraCalibration.yml"},
+	{RAND_CAMERA_ID, "../camera-config/RandCameraCalibration.yml"},
+	{CJ1_CAMERA_ID, "../camera-config/CJ1.yml"},
+	{BOX_CAMERA_ID, "../camera-config/BoxCamera.yml"},
+	{CJ3_CAMERA_ID, "../camera-config/CJ3.yml"},
+	{DRILL_CAMERA_ID, "../camera-config/DrillCamera.yml"},
+	{MICROSCOPE_ID, "../camera-config/Microscope.yml"},
 };
 
 /**

src/camera/CMakeLists.txt

@@ -12,12 +12,13 @@ find_package(Threads REQUIRED)
 add_executable(calibration
   calibration.cpp)
 
-add_executable(panoramic
-  ./CameraParams.cpp
-  ./Camera.cpp
-  ./CameraConfig.cpp
-  PanoramicCamera.cpp)
-
 target_link_libraries(calibration ${OpenCV_LIBS})
-target_link_libraries(panoramic ${OpenCV_LIBS} can_interface real_world_interface)
 
+if(WORLD_INTERFACE STREQUAL "REAL")
+  add_executable(panoramic
+    ./CameraParams.cpp
+    ./Camera.cpp
+    ./CameraConfig.cpp
+    PanoramicCamera.cpp)
+  target_link_libraries(panoramic ${OpenCV_LIBS} can_interface real_world_interface)
+endif()

src/control_interface.cpp

@@ -186,22 +186,21 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) {
 		setMotorPower(motorid_t::wristDiffLeft, gearPwr.left);
 		setMotorPower(motorid_t::wristDiffRight, gearPwr.right);
 	} else if (joint == jointid_t::fourBarLinkage) {
-    auto dp = getJointPos(jointid_t::fourBarLinkage);
-    if (dp.isValid()) {
-      int currAngle = dp.getData();
-      currAngle = currAngle / 1000;
-      if(currAngle > 320 || 
-        currAngle < 50) {
-        setMotorPower(motorid_t::fourbar1, power * 0.7);
-        setMotorPower(motorid_t::fourbar2, power * 0.7);
-      } else if (currAngle < 265 && power < 0){
-        setMotorPower(motorid_t::fourbar1, -power);
-        setMotorPower(motorid_t::fourbar2, -power);
-      } else {
-        setMotorPower(motorid_t::fourbar1, power);
-        setMotorPower(motorid_t::fourbar2, power);
-      }
-    } else {
+		auto dp = getJointPos(jointid_t::fourBarLinkage);
+		if (dp.isValid()) {
+			int currAngle = dp.getData();
+			currAngle = currAngle / 1000;
+			if (currAngle > 320 || currAngle < 50) {
+				setMotorPower(motorid_t::fourbar1, power * 0.7);
+				setMotorPower(motorid_t::fourbar2, power * 0.7);
+			} else if (currAngle < 265 && power < 0) {
+				setMotorPower(motorid_t::fourbar1, -power);
+				setMotorPower(motorid_t::fourbar2, -power);
+			} else {
+				setMotorPower(motorid_t::fourbar1, power);
+				setMotorPower(motorid_t::fourbar2, power);
+			}
+		} else {
 			setMotorPower(motorid_t::fourbar1, power);
 			setMotorPower(motorid_t::fourbar2, power);
 		}

src/navtypes.h

@@ -31,11 +31,14 @@ using pose_t = Eigen::Vector3d;		 // Robot pose: x, y, theta
 using transform_t = Eigen::Matrix3d; // a pose in matrix form; see toPose below
 using trajectory_t = std::vector<transform_t>;
 
-template <int rows, int cols> using Matrixd = Eigen::Matrix<double, rows, cols>;
+template <int rows, int cols>
+using Matrixd = Eigen::Matrix<double, rows, cols>;
 
-template <int dim> using Vectord = Eigen::Matrix<double, dim, 1>;
+template <int dim>
+using Vectord = Eigen::Matrix<double, dim, 1>;
 
-template <int dim, int dim2 = 1> using Arrayd = Eigen::Array<double, dim, dim2>;
+template <int dim, int dim2 = 1>
+using Arrayd = Eigen::Array<double, dim, dim2>;
 
 using point_t =
 	Eigen::Vector3d; // x, y, 1 (or 0, 0, 0 to represent "no data")

src/world_interface/simulator_interface.cpp

@@ -87,10 +87,6 @@ void sendJSON(const json& obj) {
 	wsServer->get().sendJSON(PROTOCOL_PATH, obj);
 }
 
-static std::shared_ptr<robot::types::CameraHandle> openCamera(CameraID cam) {
-	return nullptr;
-}
-
 static std::shared_ptr<robot::types::CameraHandle> openCamera(CameraID cam, std::optional<std::vector<double>> list1d = std::nullopt,
 					   uint8_t fps = 20, uint16_t width = 640, uint16_t height = 480) {
 	if (list1d) {
@@ -114,14 +110,6 @@ static std::shared_ptr<robot::types::CameraHandle> openCamera(CameraID cam, std:
 	return nullptr;
 }
 
-void initCameras() {
-	auto cfg = cam::readConfigFromFile(Constants::CAMERA_CONFIG_PATHS.at(Constants::MAST_CAMERA_ID));
-	cameraConfigMap[Constants::MAST_CAMERA_ID] = cfg;
-	openCamera(Constants::HAND_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList());
-	openCamera(Constants::WRIST_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList());
-	openCamera(Constants::MAST_CAMERA_ID, cfg.intrinsicParams->getIntrinsicList());
-}
-
 void initMotors() {
 	// initializes map of motor ids and shared ptrs of their objects
 	for (const auto& x : motorNameMap) {
@@ -265,7 +253,6 @@ void world_interface_init(
 	std::optional<std::reference_wrapper<net::websocket::SingleClientWSServer>> wsServer,
 	bool initOnlyMotors) {
 	initSimServer(wsServer.value());
-	initCameras();
 	initMotors();
 }
 
@@ -298,6 +285,27 @@ std::unordered_set<CameraID> getCameras() {
 	return util::keySet(cameraLastFrameIdxMap);
 }
 
+std::shared_ptr<robot::types::CameraHandle> openCamera(CameraID cam) {
+	cv::FileStorage fs(Constants::CAMERA_CONFIG_PATHS.at(cam), cv::FileStorage::READ);
+	if (!fs.isOpened()) {
+		throw std::invalid_argument("Configuration file for Camera ID" + std::to_string(cam) + " does not exist");
+	}
+
+	if (fs[KEY_IMAGE_WIDTH].empty() || fs[KEY_IMAGE_HEIGHT].empty() || fs[KEY_FRAMERATE].empty()) {
+		throw std::invalid_argument("Configuration file missing key(s)");
+	}
+
+	json msg = {{"type", "simCameraStreamOpenRequest"},
+			{"camera", cam},
+			{"fps", static_cast<int>(fs[KEY_FRAMERATE])},
+			{"width", static_cast<int>(fs[KEY_IMAGE_WIDTH])},
+			{"height", static_cast<int>(fs[KEY_IMAGE_HEIGHT])},
+			{"intrinsics", nullptr}};
+	sendJSON(msg);
+
+	return nullptr;
+}
+
 bool hasNewCameraFrame(CameraID cameraID, uint32_t oldFrameNum) {
 	// acquire shared lock
 	std::shared_lock<std::shared_mutex> lock(cameraFrameMapMutex);
@@ -392,6 +400,18 @@ void setMotorVel(robot::types::motorid_t motor, int32_t targetVel) {
 	motor_ptr->setMotorVel(targetVel);
 }
 
+void setServoPos(robot::types::servoid_t servo, int32_t position) {
+	// Implement when Servos are added to Simulator
+}
+
+void setRequestedStepperTurnAngle(robot::types::stepperid_t stepper, int16_t angle) {
+	// Implement when Steppers are added to Simulator
+}
+
+void setActuator(uint8_t value) {
+	// Implement when Actuators are added to Simulator
+}
+
 callbackid_t addLimitSwitchCallback(
 	robot::types::motorid_t motor,
 	const std::function<void(robot::types::motorid_t motor,