Fix CMake circular dependencies (#326)

* Remove E_STOP from globals * Add control interface and remove more dependencies on globals * minor cmake fix * Fix format * Fix mismatching parameter name
huskyroboticsteam · May 25, 2024 · 4c00005 · 4c00005
1 parent 11fac86
commit 4c00005
Show file tree

Hide file tree

Showing 16 changed files with 132 additions and 73 deletions.
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
@@ -190,7 +190,7 @@ elseif(GPS STREQUAL "ARDUPILOT")
   target_sources(gps_interface PUBLIC
       ardupilot/ArduPilotProtocol.cpp
       ardupilot/ArduPilotInterface.cpp)
-  target_link_libraries(gps_interface utils websocket_utils rover_common)
+  target_link_libraries(gps_interface utils websocket_utils)
 else()
   target_sources(gps_interface PUBLIC gps/dummy/dummy_gps.cpp)
 endif()
@@ -199,7 +199,6 @@ endif()
 # hardware-agnostic utilities and common code for world interface
 add_library(world_interface_core STATIC
   world_interface/gps_common_interface.cpp
-  world_interface/kinematic_common_interface.cpp
   ar/Detector.cpp
   ar/MarkerSet.cpp
   ar/MarkerPattern.cpp
@@ -219,17 +218,25 @@ add_library(stub_world_interface STATIC
 add_library(real_world_interface STATIC
   world_interface/real_world_interface.cpp
   world_interface/motor/can_motor.cpp
-  Globals.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")
+  add_library(world_interface ALIAS real_world_interface)
+elseif(WORLD_INTERFACE STREQUAL "SIMULATOR")
+  add_library(world_interface ALIAS simulator_interface)
+else()
+  add_library(world_interface ALIAS stub_world_interface)
+endif()
+
 add_library(filters SHARED
   filters/StateSpaceUtil.cpp
 )
@@ -255,21 +262,21 @@ add_library(commands SHARED
 
 add_library(autonomous SHARED
   autonomous/AutonomousTask.cpp)
-target_link_libraries(autonomous commands)
+target_link_libraries(autonomous commands world_interface)
 
-list(APPEND simulator_libs
-  simulator_interface)
+add_library(constants SHARED Constants.cpp)
+link_libraries(constants)
 
 add_executable(Rover Rover.cpp)
 add_library(rover_common SHARED
-  Constants.cpp
   Globals.cpp
-  network/websocket/WebSocketServer.cpp
-  network/websocket/WebSocketProtocol.cpp
+  control_interface.cpp
   )
 target_link_libraries(rover_common
   utils
-  mission_control_interface)
+  websocket_utils
+  world_interface
+)
 
 list(APPEND rover_libs
   rover_common
@@ -281,23 +288,17 @@ list(APPEND rover_libs
   utils
 )
 target_link_libraries(Rover
-  ${rover_libs})
-
-if(WORLD_INTERFACE STREQUAL "REAL")
-  target_link_libraries(Rover real_world_interface)
-elseif(WORLD_INTERFACE STREQUAL "SIMULATOR")
-  target_link_libraries(Rover ${simulator_libs})
-else()
-  target_link_libraries(Rover real_world_interface) # TODO: support other interfaces
-endif()
-
-target_link_libraries(Rover ${vision_libs})
+  ${rover_libs}
+  mission_control_interface
+  world_interface
+  ${vision_libs}
+)
 
 add_executable(TunePID TunePID.cpp)
 target_link_libraries(TunePID
   ${rover_libs}
   can_interface
-  real_world_interface
+  world_interface
 )
 target_link_libraries(TunePID ${vision_libs})
 

diff --git a/src/Globals.cpp b/src/Globals.cpp
@@ -44,7 +44,6 @@ namespace Globals {
 RoverState curr_state;
 net::websocket::SingleClientWSServer websocketServer("DefaultServer",
 													 Constants::WS_SERVER_PORT);
-std::atomic<bool> E_STOP = false;
 std::atomic<bool> AUTONOMOUS = false;
 robot::types::mountedperipheral_t mountedPeripheral = robot::types::mountedperipheral_t::none;
 control::PlanarArmController<2> planarArmController(createArmKinematics(),

diff --git a/src/Globals.h b/src/Globals.h
@@ -25,7 +25,6 @@ namespace Globals {
  */
 extern RoverState curr_state;
 extern net::websocket::SingleClientWSServer websocketServer;
-extern std::atomic<bool> E_STOP;
 extern std::atomic<bool> AUTONOMOUS;
 extern robot::types::mountedperipheral_t mountedPeripheral;
 extern control::PlanarArmController<2> planarArmController;

diff --git a/src/LimitCalib.cpp b/src/LimitCalib.cpp
@@ -34,7 +34,7 @@ void cleanup(int signum) {
 // Then begins to run the motors.
 int main() {
 	// init motors.
-	robot::world_interface_init(true);
+	robot::world_interface_init(std::nullopt, true);
 
 	signal(SIGINT, cleanup);
 

diff --git a/src/Rover.cpp b/src/Rover.cpp
@@ -26,7 +26,7 @@ using std::chrono::steady_clock;
 using namespace navtypes;
 using namespace robot::types;
 
-void closeRover(int signum) {
+void closeRover(int) {
 	robot::emergencyStop();
 	Globals::websocketServer.stop();
 	raise(SIGTERM);
@@ -149,7 +149,7 @@ int main(int argc, char** argv) {
 	parseCommandLine(argc, argv);
 	Globals::AUTONOMOUS = false;
 	Globals::websocketServer.start();
-	robot::world_interface_init();
+	robot::world_interface_init(Globals::websocketServer);
 	auto mcProto = std::make_unique<net::mc::MissionControlProtocol>(Globals::websocketServer);
 	Globals::websocketServer.addProtocol(std::move(mcProto));
 	// Ctrl+C doesn't stop the simulation without this line

diff --git a/src/TunePID.cpp b/src/TunePID.cpp
@@ -55,7 +55,7 @@ void cleanup(int signum) {
 }
 
 int main(int argc, char** argv) {
-	robot::world_interface_init();
+	robot::world_interface_init(std::nullopt);
 
 	// TODO: Before running this script, make sure the PPJR is set correctly for each motor
 	// in real_world_constants.cpp

diff --git a/src/ardupilot/ArduPilotProtocol.cpp b/src/ardupilot/ArduPilotProtocol.cpp
@@ -1,7 +1,6 @@
 #include "ArduPilotProtocol.h"
 
 #include "../Constants.h"
-#include "../Globals.h"
 #include "../utils/json.h"
 #include "../utils/transform.h"
 

diff --git a/src/autonomous/AutonomousTask.cpp b/src/autonomous/AutonomousTask.cpp
@@ -2,6 +2,7 @@
 
 #include "../Constants.h"
 #include "../commands/DriveToWaypointCommand.h"
+#include "../control_interface.h"
 #include "../utils/transform.h"
 #include "../world_interface/world_interface.h"
 

diff --git a/..._interface/kinematic_common_interface.cpp → src/control_interface.cpp b/..._interface/kinematic_common_interface.cpp → src/control_interface.cpp
@@ -1,9 +1,11 @@
-#include "../Constants.h"
-#include "../Globals.h"
-#include "../kinematics/DiffDriveKinematics.h"
-#include "../navtypes.h"
-#include "../utils/transform.h"
-#include "world_interface.h"
+#include "control_interface.h"
+
+#include "Constants.h"
+#include "Globals.h"
+#include "kinematics/DiffDriveKinematics.h"
+#include "navtypes.h"
+#include "utils/transform.h"
+#include "world_interface/world_interface.h"
 
 #include <atomic>
 #include <chrono>
@@ -32,7 +34,7 @@ double getJointPowerValue(types::jointid_t joint);
 } // namespace
 
 double setCmdVel(double dtheta, double dx) {
-	if (Globals::E_STOP && (dtheta != 0 || dx != 0)) {
+	if (isEmergencyStopped()) {
 		return 0;
 	}
 

diff --git a/src/control_interface.h b/src/control_interface.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include "world_interface/data.h"
+
+// This is a layer that provides further abstraction on top of
+// the motor-level control provided by the world interface
+
+namespace robot {
+
+/**
+ * @brief Request the robot to drive at the given velocities.
+ *
+ * @param dtheta The heading velocity.
+ * @param dx The forward velocity.
+ * @return double If the requested velocities are too high, they will be scaled down.
+ * The returned value is the scale divisor. If no scaling was performed, 1 is returned.
+ */
+double setCmdVel(double dtheta, double dx);
+
+/**
+ * @brief Set the power of the specified joint.
+ *
+ * @param joint The joint to set the power of.
+ * @param power The power value to set, in the range [-1,1].
+ */
+void setJointPower(types::jointid_t joint, double power);
+
+/**
+ * @brief Set the position of the specified joint.
+ *
+ * @param joint the jointid_t of the joint to set the position of.
+ * @param targetPos the position to set the joint to, in millidegrees.
+ */
+void setJointPos(types::jointid_t joint, int32_t targetPos);
+
+/**
+ * @param joint the jointid_t of the joint to get the position of.
+ * @return the position of the joint specified by the jointid_t argument joint,
+ * in millidegrees.
+ */
+types::DataPoint<int32_t> getJointPos(types::jointid_t joint);
+
+} // namespace robot
diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp
@@ -3,6 +3,7 @@
 #include "../Constants.h"
 #include "../Globals.h"
 #include "../autonomous/AutonomousTask.h"
+#include "../control_interface.h"
 #include "../utils/core.h"
 #include "../utils/json.h"
 #include "../world_interface/data.h"
@@ -56,7 +57,6 @@ void MissionControlProtocol::handleEmergencyStopRequest(const json& j) {
 		_power_repeat_task.start();
 	}
 	// TODO: reinit motors
-	Globals::E_STOP = stop;
 	this->setArmIKEnabled(false);
 }
 
@@ -245,7 +245,6 @@ void MissionControlProtocol::handleHeartbeatTimedOut() {
 	LOG_F(ERROR, "Heartbeat timed out! Emergency stopping.");
 	this->stopAndShutdownPowerRepeat(true);
 	robot::emergencyStop();
-	Globals::E_STOP = true;
 }
 
 void MissionControlProtocol::stopAndShutdownPowerRepeat(bool sendDisableIK) {

diff --git a/src/network/MissionControlTasks.cpp b/src/network/MissionControlTasks.cpp
@@ -2,6 +2,7 @@
 
 #include "../Constants.h"
 #include "../Globals.h"
+#include "../control_interface.h"
 #include "../utils/core.h"
 #include "../world_interface/world_interface.h"
 #include "MissionControlMessages.h"

diff --git a/src/world_interface/noop_world.cpp b/src/world_interface/noop_world.cpp
@@ -27,14 +27,20 @@ const DiffWristKinematics& wristKinematics() {
 	return wrist_kinematics;
 }
 
-void world_interface_init(bool initMotorsOnly) {}
+void world_interface_init(
+	std::optional<std::reference_wrapper<net::websocket::SingleClientWSServer>> wsServer,
+	bool initMotorsOnly) {}
 
 std::shared_ptr<robot::base_motor> getMotor(robot::types::motorid_t motor) {
 	return nullptr;
 }
 
 void emergencyStop() {}
 
+bool isEmergencyStopped() {
+	return false;
+}
+
 landmarks_t readLandmarks() {
 	return landmarks_t{};
 }

diff --git a/src/world_interface/real_world_interface.cpp b/src/world_interface/real_world_interface.cpp
@@ -2,7 +2,6 @@
 #include "../CAN/CANMotor.h"
 #include "../CAN/CANUtils.h"
 #include "../Constants.h"
-#include "../Globals.h"
 #include "../ardupilot/ArduPilotInterface.h"
 #include "../camera/Camera.h"
 #include "../gps/usb_gps/read_usb_gps.h"
@@ -35,6 +34,7 @@ std::unordered_map<robot::types::motorid_t, std::shared_ptr<robot::base_motor>>
 namespace {
 kinematics::DiffDriveKinematics drive_kinematics(Constants::EFF_WHEEL_BASE);
 kinematics::DiffWristKinematics wrist_kinematics;
+bool is_emergency_stopped = false;
 
 void addMotorMapping(motorid_t motor, bool hasPosSensor) {
 	// get scales for motor
@@ -133,10 +133,14 @@ void setupCameras() {
 }
 } // namespace
 
-void world_interface_init(bool initOnlyMotors) {
+void world_interface_init(
+	std::optional<std::reference_wrapper<net::websocket::SingleClientWSServer>> wsServer,
+	bool initOnlyMotors) {
 	if (!initOnlyMotors) {
 		setupCameras();
-		ardupilot::initArduPilotProtocol(Globals::websocketServer);
+		if (wsServer.has_value()) {
+			ardupilot::initArduPilotProtocol(wsServer.value());
+		}
 	}
 	can::initCAN();
 	initMotors();
@@ -157,6 +161,11 @@ std::shared_ptr<robot::base_motor> getMotor(robot::types::motorid_t motor) {
 
 void emergencyStop() {
 	can::motor::emergencyStopMotors();
+	is_emergency_stopped = true;
+}
+
+bool isEmergencyStopped() {
+	return is_emergency_stopped;
 }
 
 std::unordered_set<CameraID> getCameras() {