diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp index 061e28a6..771b7089 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -86,10 +86,7 @@ void MissionControlProtocol::handleEmergencyStopRequest(const json& j) { } // TODO: reinit motors Globals::E_STOP = stop; - Globals::armIKEnabled = false; - - // inform MC that Ik has been disabled - sendArmIKEnabledReport(); + this->setArmIKEnabled(); } static bool validateOperationModeRequest(const json& j) { @@ -136,29 +133,29 @@ void MissionControlProtocol::handleDriveRequest(const json& j) { void MissionControlProtocol::handleRequestArmIKEnabled(const json& j) { bool enabled = j["enabled"]; if (enabled) { - Globals::armIKEnabled = false; - if (_arm_ik_repeat_thread.joinable()) { - _arm_ik_repeat_thread.join(); - } - DataPoint> armJointPositions = - robot::getMotorPositionsRad(Constants::arm::IK_MOTORS); - // Rover responds with armIKEnabledReport after requestArmIKEnable is processed - if (armJointPositions.isValid()) { - Globals::planarArmController.set_setpoint(armJointPositions.getData()); - Globals::armIKEnabled = true; - _arm_ik_repeat_thread = - std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this); - } else { - // unable to enable IK - log(LOG_WARN, "Unable to enable IK"); + if (!Globals::armIKEnabled) { + if (_arm_ik_repeat_thread.joinable()) { + _arm_ik_repeat_thread.join(); + } + DataPoint> armJointPositions = + robot::getMotorPositionsRad(Constants::arm::IK_MOTORS); + // Rover responds with armIKEnabledReport after requestArmIKEnable is processed + if (armJointPositions.isValid()) { + Globals::planarArmController.set_setpoint(armJointPositions.getData()); + this->setArmIKEnabled(true); + _arm_ik_repeat_thread = + std::thread(&MissionControlProtocol::updateArmIKRepeatTask, this); + } else { + // unable to enable IK + log(LOG_WARN, "Unable to enable IK"); + } } } else { - Globals::armIKEnabled = false; + this->setArmIKEnabled(false); if (_arm_ik_repeat_thread.joinable()) { _arm_ik_repeat_thread.join(); } } - sendArmIKEnabledReport(); } void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) { @@ -283,7 +280,7 @@ void MissionControlProtocol::sendCameraStreamReport( void MissionControlProtocol::handleConnection() { // Turn off inverse kinematics on connection - Globals::armIKEnabled = false; + this->setArmIKEnabled(false); // TODO: send the actual mounted peripheral, as specified by the command-line parameter json j = {{"type", MOUNTED_PERIPHERAL_REP_TYPE}}; @@ -295,7 +292,6 @@ void MissionControlProtocol::handleConnection() { } this->_server.sendJSON(Constants::MC_PROTOCOL_NAME, j); - sendArmIKEnabledReport(); if (!Globals::AUTONOMOUS) { // start power repeat thread (if not already running) @@ -308,7 +304,6 @@ void MissionControlProtocol::handleHeartbeatTimedOut() { robot::emergencyStop(); LOG_F(ERROR, "Heartbeat timed out! Emergency stopping."); Globals::E_STOP = true; - Globals::armIKEnabled = false; } void MissionControlProtocol::startPowerRepeat() { @@ -351,7 +346,7 @@ void MissionControlProtocol::stopAndShutdownPowerRepeat() { } } // Turn off inverse kinematics so that IK state will be in sync with mission control - Globals::armIKEnabled = false; + this->setArmIKEnabled(false); } MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server) @@ -423,6 +418,11 @@ MissionControlProtocol::~MissionControlProtocol() { } } +void MissionControlProtocol::setArmIKEnabled(const bool enabled) { + Globals::armIKEnabled = enabled; + this->sendArmIKEnabledReport(); +} + void MissionControlProtocol::setRequestedJointPower(jointid_t joint, double power) { std::lock_guard joint_lock(this->_joint_power_mutex); this->_last_joint_power[joint] = power; diff --git a/src/network/MissionControlProtocol.h b/src/network/MissionControlProtocol.h index 2fc7a850..f71042cc 100644 --- a/src/network/MissionControlProtocol.h +++ b/src/network/MissionControlProtocol.h @@ -73,6 +73,7 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta void handleHeartbeatTimedOut(); void startPowerRepeat(); void stopAndShutdownPowerRepeat(); + void setArmIKEnabled(const bool enabled); void setRequestedJointPower(jointid_t joint, double power); void setRequestedCmdVel(double dtheta, double dx); };