void ServoMgr::servoSubscriber()

in servo_pkg/src/servo_mgr.cpp [68:107]
36 lines of code
9 McCabe index (conditional complexity)

    void ServoMgr::servoSubscriber(const deepracer_interfaces_pkg::msg::ServoCtrlMsg::SharedPtr servoMsg) {
        auto setPWM = [&](auto &servo, auto value, auto type) {
            if (value < -1.0 || value > 1.0) {
                RCLCPP_ERROR(logger_, "Invalid servo request: %d", value);
                return;
            }

            auto itCal = calibrationMap_.find(type);
            if (itCal == calibrationMap_.end()) {
                RCLCPP_ERROR(logger_, "Invalid calibration type");
                return;
            }
            auto itMin = itCal->second.find(MIN_KEY);
            auto itMid = itCal->second.find(MID_KEY);
            auto itMax = itCal->second.find(MAX_KEY);
            auto itPolar = itCal->second.find(POLARITY_KEY);

            if (itMin != itCal->second.end() && itMid != itCal->second.end() 
                && itMax != itCal->second.end() && itPolar != itCal->second.end()) {
                float adjVal = value * itPolar->second;
                if (adjVal < 0) {
                    servo->setDuty(itMid->second + adjVal * (itMid->second - itMin->second));
                }
                else if (adjVal > 0) {
                        servo->setDuty(itMid->second + adjVal * (itMax->second - itMid->second));
                }
                else {
                    servo->setDuty(itMid->second);
                }
            }
            else {
                RCLCPP_ERROR(logger_, "Invalid calibrations");
            }
        };

        setPWM(throttle_, servoMsg->throttle, motor);
        setPWM(angle_, servoMsg->angle, servo);
        // Make sure that the pulse goes through a full period
        std::this_thread::sleep_for(std::chrono::nanoseconds(SERVO_PERIOD));
    }