From 4c0000594c3e68959f21f2c5806ba46d188e7143 Mon Sep 17 00:00:00 2001 From: Abhay Deshpande Date: Fri, 24 May 2024 22:08:22 -0700 Subject: [PATCH] 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 --- src/CMakeLists.txt | 49 ++++++++++--------- src/Globals.cpp | 1 - src/Globals.h | 1 - src/LimitCalib.cpp | 2 +- src/Rover.cpp | 4 +- src/TunePID.cpp | 2 +- src/ardupilot/ArduPilotProtocol.cpp | 1 - src/autonomous/AutonomousTask.cpp | 1 + ...on_interface.cpp => control_interface.cpp} | 16 +++--- src/control_interface.h | 43 ++++++++++++++++ src/network/MissionControlProtocol.cpp | 3 +- src/network/MissionControlTasks.cpp | 1 + src/world_interface/noop_world.cpp | 8 ++- src/world_interface/real_world_interface.cpp | 15 ++++-- src/world_interface/simulator_interface.cpp | 23 ++++++--- src/world_interface/world_interface.h | 35 +++++-------- 16 files changed, 132 insertions(+), 73 deletions(-) rename src/{world_interface/kinematic_common_interface.cpp => control_interface.cpp} (95%) create mode 100644 src/control_interface.h diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1433f881..b51e24bd 100644 --- 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 index 37ee1063..40f5ae0a 100644 --- 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 E_STOP = false; std::atomic 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 index 5427d7d8..6ad315ff 100644 --- 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 E_STOP; extern std::atomic AUTONOMOUS; extern robot::types::mountedperipheral_t mountedPeripheral; extern control::PlanarArmController<2> planarArmController; diff --git a/src/LimitCalib.cpp b/src/LimitCalib.cpp index f6796da3..93593b1e 100644 --- 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 index f349e2dc..2ce8d2e2 100644 --- 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(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 index ecaf60f7..f75eac61 100644 --- 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 index db503449..b8108b4b 100644 --- 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 index 3ca02d09..e3067abe 100644 --- 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/src/world_interface/kinematic_common_interface.cpp b/src/control_interface.cpp similarity index 95% rename from src/world_interface/kinematic_common_interface.cpp rename to src/control_interface.cpp index 91c4e651..c48299d0 100644 --- a/src/world_interface/kinematic_common_interface.cpp +++ b/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 #include @@ -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 new file mode 100644 index 00000000..218ed38c --- /dev/null +++ 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 getJointPos(types::jointid_t joint); + +} // namespace robot \ No newline at end of file diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index a19649e5..eb248cc4 100644 --- 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 index 3d936ec1..860545c7 100644 --- 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 index 22743c80..0f100e54 100644 --- a/src/world_interface/noop_world.cpp +++ b/src/world_interface/noop_world.cpp @@ -27,7 +27,9 @@ const DiffWristKinematics& wristKinematics() { return wrist_kinematics; } -void world_interface_init(bool initMotorsOnly) {} +void world_interface_init( + std::optional> wsServer, + bool initMotorsOnly) {} std::shared_ptr getMotor(robot::types::motorid_t motor) { return nullptr; @@ -35,6 +37,10 @@ std::shared_ptr getMotor(robot::types::motorid_t motor) { 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 index 5f2f7d57..a94c9dbe 100644 --- 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> 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> 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 getMotor(robot::types::motorid_t motor) { void emergencyStop() { can::motor::emergencyStopMotors(); + is_emergency_stopped = true; +} + +bool isEmergencyStopped() { + return is_emergency_stopped; } std::unordered_set getCameras() { diff --git a/src/world_interface/simulator_interface.cpp b/src/world_interface/simulator_interface.cpp index 995dce5a..5056085a 100644 --- a/src/world_interface/simulator_interface.cpp +++ b/src/world_interface/simulator_interface.cpp @@ -1,5 +1,4 @@ #include "../Constants.h" -#include "../Globals.h" #include "../base64/base64_img.h" #include "../camera/Camera.h" #include "../camera/CameraConfig.h" @@ -42,6 +41,8 @@ const std::map motorNameMap = { {motorid_t::hand, "hand"}, {motorid_t::activeSuspension, "activeSuspension"}}; +std::optional> wsServer; + DataPoint lastOrientation; std::mutex orientationMutex; @@ -54,6 +55,8 @@ std::mutex truePoseMutex; std::map> motorPosMap; std::shared_mutex motorPosMapMutex; +bool is_emergency_stopped = false; + // A mapping of (motor_id, shared pointer to object of the motor) std::unordered_map> motor_ptrs; @@ -79,7 +82,7 @@ std::mutex connectionMutex; bool simConnected = false; void sendJSON(const json& obj) { - Globals::websocketServer.sendJSON(PROTOCOL_PATH, obj); + wsServer->get().sendJSON(PROTOCOL_PATH, obj); } static void openCamera(CameraID cam, std::optional> list1d = std::nullopt, @@ -215,7 +218,8 @@ void clientDisconnected() { LOG_F(ERROR, "ERROR: Simulator disconnected! World Interface disconnected!"); } -void initSimServer() { +void initSimServer(net::websocket::SingleClientWSServer& ws) { + wsServer = ws; auto protocol = std::make_unique(PROTOCOL_PATH); protocol->addMessageHandler("simImuOrientationReport", handleIMU); protocol->addMessageHandler("simGpsPositionReport", handleGPS); @@ -226,7 +230,7 @@ void initSimServer() { protocol->addConnectionHandler(clientConnected); protocol->addDisconnectionHandler(clientDisconnected); - Globals::websocketServer.addProtocol(std::move(protocol)); + wsServer->get().addProtocol(std::move(protocol)); { // wait for simulator to connect @@ -255,8 +259,10 @@ const DiffWristKinematics& wristKinematics() { extern const WorldInterface WORLD_INTERFACE = WorldInterface::sim3d; -void world_interface_init(bool initOnlyMotors) { - initSimServer(); +void world_interface_init( + std::optional> wsServer, + bool initOnlyMotors) { + initSimServer(wsServer.value()); initCameras(); initMotors(); } @@ -278,6 +284,11 @@ void emergencyStop() { for (const auto& motor : motorNameMap) { setMotorPower(motor.first, 0.0); } + is_emergency_stopped = true; +} + +bool isEmergencyStopped() { + return is_emergency_stopped; } std::unordered_set getCameras() { diff --git a/src/world_interface/world_interface.h b/src/world_interface/world_interface.h index b5bccfb6..b48e06bd 100644 --- a/src/world_interface/world_interface.h +++ b/src/world_interface/world_interface.h @@ -4,6 +4,7 @@ #include "../kinematics/DiffDriveKinematics.h" #include "../kinematics/DiffWristKinematics.h" #include "../navtypes.h" +#include "../network/websocket/WebSocketServer.h" #include "data.h" #include "motor/base_motor.h" @@ -43,8 +44,15 @@ const kinematics::DiffWristKinematics& wristKinematics(); * * This method should only be called once, and must be called * before any other world interface methods. + * + * @param wsServer A reference to the websocket server to use for communication. + * If empty, components that require it will not be initialized. + * @param initOnlyMotors If true, only initialize the motors and not the rest of the world + * interface. */ -void world_interface_init(bool initOnlyMotors = false); +void world_interface_init( + std::optional> wsServer, + bool initOnlyMotors = false); /** * @brief Get a pointer to the motor object associated with the motor id. @@ -62,14 +70,11 @@ std::shared_ptr getMotor(robot::types::motorid_t motor); void emergencyStop(); /** - * @brief Request the robot to drive at the given velocities. + * @brief Check if the robot has been emergency stopped. * - * @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. + * @return If emergencyStop() has been called previously. */ -double setCmdVel(double dtheta, double dx); +bool isEmergencyStopped(); /** * @brief Get the IDs of the currently supported cameras. @@ -224,22 +229,6 @@ callbackid_t addLimitSwitchCallback( void removeLimitSwitchCallback(callbackid_t id); -// TODO: document -void setJointPower(robot::types::jointid_t joint, double power); - -/** - * @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(robot::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 getJointPos(robot::types::jointid_t joint); - /** * @brief Get the positions in radians of multiple motors at the same time. *