bool ServoMgr::calibrateServo()

in servo_pkg/src/servo_mgr.cpp [297:343]
39 lines of code
9 McCabe index (conditional complexity)

    bool ServoMgr::calibrateServo(int pinNum, int pinVal) const {

        if (pinVal > 1 || pinVal < 0 || pinNum < 0) {
            RCLCPP_ERROR(logger_, "Invalid pin values");
            return false;
        }

        const int MAX_BUF = 64;
        auto writeGPIO = [&](const std::string &path, int pinNum,
                                const std::string &value) {
            char pinBuff[MAX_BUF];
            snprintf(pinBuff, sizeof(pinBuff), path.c_str(), pinNum);
            RCLCPP_INFO(logger_,"%s", value.c_str());
            int fd = open(pinBuff, O_WRONLY);
            if (fd < 0) {
                RCLCPP_ERROR(logger_, "Failed to open: %s", path.c_str());
                return false;
            }
            char valueBuff[MAX_BUF];
            write(fd, valueBuff, snprintf(valueBuff, sizeof(valueBuff),
                                            "%s", value.c_str()));
            close(fd);
            return true;
        };
        bool ret = true;
        // Open the pin path and check its existence, if not create it.
        char buff[MAX_BUF];
        snprintf(buff, sizeof(buff), "/sys/class/gpio/gpio%d", pinNum);
        int gpioFd = open(buff, O_WRONLY);
        if (gpioFd < 0) {
            // Open the export path to export the pin.
            ret = ret && writeGPIO("/sys/class/gpio/export", pinNum, std::to_string(pinNum));
        }
        else {
            close(gpioFd);
        }
        // Write the direction, it is always "out"
        ret = ret && writeGPIO("/sys/class/gpio/gpio%d/direction", pinNum, "out");
        // Set the value, 0 for enable and 1 for disable.
        ret = ret && writeGPIO("/sys/class/gpio/gpio%d/value", pinNum, std::to_string(pinVal));

        memset(buff, 0, sizeof(buff));
        snprintf(buff, sizeof(buff), "/sys/class/gpio/gpio%d/value", pinNum);
        chmod(buff, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);

        return ret;
    }