Linux中ROS风格的物理PWM引脚控制，C++代码

背景：拿到一个舵机，一个安装了linux和ROS的“小黑盒子”。现在只是能跑的状态，提供一种思路，之后我还得大改代码。
注意：文件的读写需要权限，在运行节点之前，需要

su root

需要知道哪些：
舵机控制原理
ROS话题通信
对应开发板的引脚定义

因为Linux里面一切都是文件，所以驱动爪子只需要改对应文件的值就可以了。

#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include "ros/ros.h"
#include "std_msgs/Int32.h"

static char pwm_path[100];
static int pwm_config(const char *attr, const char *val)    //配置PWM
{
    char file_path[200];
    int len;
    int fd;
    sprintf(file_path, "%s/%s", pwm_path, attr);
    if (0 > (fd = open(file_path, O_WRONLY, S_IWUSR))) {
        perror("open error");
        return fd;
    }
    len = strlen(val);
    if (len != write(fd, val, len)) {
        ROS_INFO("erro in %s", attr);
        perror("write error");
        close(fd);
    return -1;
    }
    close(fd); //关闭文件
    return 0;
}
 
bool drive(const std_msgs::Int32::ConstPtr &duty){
    char duty_s[16];//将输入转换为字符串
    int legal_duty = 1000000;
    duty->data < 1000000 ? legal_duty = 1000000 : duty->data > 1800000? legal_duty = 1800000 : legal_duty = duty->data;
    sprintf(duty_s, "%d", legal_duty);
    if (pwm_config("duty_cycle", duty_s)){
        ROS_INFO("Change duty cycle failed.");
        return false;
    }
    ROS_INFO("duty:<%s>",duty_s);
    return true;
}
 
int main(int argc, char *argv[])
{
    /*启动pwm节点，需要传入三个参数*/
    /* 校验传参 */
    if (4 != argc) {//4 20,000,000 20000000 单位：纳秒 //1ms = 1,000,000 ns
    // duty 500,000 - 2,500,000 但是咱们的爪子转角并不能让舵机转满，所以最好是1,000,000 - 1,800,000
        ROS_INFO("rosrun gripper gripper_driver <id> <period> <duty>\nlegal_duty:1000000 - 1800000");
        exit(-1);
    }
    /* 打印配置信息 */
    ROS_INFO("PWM config: id:<%s>, period:<%s>, duty:<%s>\n",
    argv[1], argv[2],
    argv[3]);
    /* 导出 pwm */
    sprintf(pwm_path, "/sys/class/pwm/pwmchip%s/pwm1", argv[1]);
    if (access(pwm_path, F_OK)) {//如果 pwm1 目录不存在, 则导出
        char temp[100];
        int fd;
        sprintf(temp, "/sys/class/pwm/pwmchip%s/export", argv[1]);
        if (0 > (fd = open(temp, O_WRONLY, S_IWUSR))) {
            perror("open export error");
            exit(-1);
        }
        if (1 != write(fd, "1", 1)) {//导出 pwm
            perror("write export error");
            close(fd);
            exit(-1);
        }
        close(fd); //关闭文件
    }
    /* 配置 PWM 周期 */
    if (pwm_config("period", argv[2]))
        exit(-1);
    /* 配置占空比 */
    if (pwm_config("duty_cycle", argv[3]))
        exit(-1);
    /* 使能 pwm */
    pwm_config("enable", "1");
    /*配置完成*/
    ROS_INFO("PWM OK");

    /*订阅*/
    setlocale(LC_ALL,"");//解决中文乱码问题
    //2.初始化 ROS 节点:命名(唯一)
    ros::init(argc,argv,"gripper_driver");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;

    //4.实例化 订阅者 对象
    ros::Subscriber sub = nh.subscribe<std_msgs::Int32>("gripper_duty",2,drive);
    //5.处理订阅的消息(回调函数)
    ros::spin();//循环读取接收的数据，并调用回调函数处理
    /* 退出程序 */
    exit(0);

}