carla与g29联合调试(二)

前言：

对于力反馈的源码解析。

一、工作空间分析

工作空间如下图所示：

config文件中是对相关参数的设定；

launch文件是将yaml文件和执行文件融合启动；

msg文件是自定义消息类型；

script文件是python版本的ros执行文件，主要是为了发送控制旋转和力度的控制指令与carla的联合调试就是在这里实现的；

src里为核心内容通过输入输出信号控制g29。

二、src解析

其他文件基础内容，只对于核心内容解析分析。

2.1 头文件

#include <ros/ros.h>
#include <linux/input.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <fcntl.h>
#include <math.h>

#include "g29_force_feedback/ForceFeedback.h"

其中：

#include <linux/input.h>

用于Linux系统中读取输入设备的信息。它提供了访问Linux内核输入子系统的接口，使开发者能够获取和处理用户输入事件，如按键、鼠标、触摸屏等。该头文件定义了一系列结构体和常量，用于描述输入设备的特性和事件。开发者可以使用这些结构体和函数来监听和处理输入事件，实现各种应用程序，例如图形界面、游戏、交互式应用等。主要用来与g29接口之间的输入输出。

#include <sys/ioctl.h>

用于进行输入输出控制操作。它提供了对设备的ioctl函数的声明，可以用来发送控制命令给设备驱动程序，实现一些特殊的操作。ioctl函数是一种特殊的系统调用，用于对已打开的设备文件进行控制。通过使用不同的参数和命令，可以实现对设备的配置、查询设备状态、发送控制命令等功能。在这个头文件中，定义了一些常量和结构体，用于ioctl函数中命令和参数的传递。程序员可以根据需要使用这些常量和结构体，调用ioctl函数来进行相应的设备控制操作。

#include "g29_force_feedback/ForceFeedback.h"

自定义消息类型

2.2 main函数

int main(int argc, char **argv)
{
    ros::init(argc, argv, "g29_force_feedback_node");
    G29ForceFeedback g29_ff;
    ros::spin();
    return(0);
}

核心在类G29ForceFeedback的实现中。

2.3 G29ForceFeedback声明

class G29ForceFeedback
{
private:
    ros::Subscriber sub_target;
    ros::Timer timer;
    float m_pub_rate;

    // variables from fouce feedback API
    int m_device_handle;
    int m_axis_code = ABS_X;
    int m_axis_min;
    int m_axis_max;
    struct ff_effect m_effect;

    // device config
    std::string m_device_name;
    double m_max_force;
    double m_min_force;

    // motion config 0:PID force, 1:constant force
    double m_Kp;
    double m_Ki;
    double m_Kd;
    double m_offset;

    // target and current state of the wheel
    bool m_pid_mode;
    double m_target_angle;
    double m_target_force;
    double m_current_angle;

public:
    G29ForceFeedback();

private:
    void targetCallback(const g29_force_feedback::ForceFeedback &in_target);
    void timerCallback(const ros::TimerEvent&);
    int testBit(int bit, unsigned char *array);
    void initFfDevice();
    void updateFfDevice();
};

逐行分析：

    ros::Subscriber sub_target;
    ros::Timer timer;
    float m_pub_rate;

一个订阅，一个计时，然后一个发布的速率。

    // variables from fouce feedback API
    int m_device_handle;
    int m_axis_code = ABS_X;
    int m_axis_min;
    int m_axis_max;
    struct ff_effect m_effect;

力反馈变量的相关设置，其中

m_device_handle：一个整数变量，表示力反馈设备的句柄或标识符。

m_axis_code：一个整数变量，表示力反馈设备的轴码，指示要应用力反馈效果的轴。

m_axis_min 和 m_axis_max：两个整数变量，表示力反馈设备的轴的最小值和最大值，用于标定力反馈效果的范围。

m_effect：一个ff_effect结构体变量，用于描述和控制力反馈效果。

而ABS_X为Linux输入子系统中定义的一个常量，用于表示力反馈设备的 X 轴。

ff_effect是Linux中的一个结构体，定义在<linux/input.h>头文件中。它用于描述和控制力反馈（force feedback）效果。

    // device config
    std::string m_device_name;
    double m_max_force;
    double m_min_force;

设备相关的设施，设备名称，最大力反馈、最小力反馈。

    // motion config 0:PID force, 1:constant force
    double m_Kp;
    double m_Ki;
    double m_Kd;
    double m_offset;

对于PID的设定。

    // target and current state of the wheel
    bool m_pid_mode;
    double m_target_angle;
    double m_target_force;
    double m_current_angle;

判断是否使用pid控制和目标角度、力反馈和当前的角度。

    void targetCallback(const g29_force_feedback::ForceFeedback &in_target);
    void timerCallback(const ros::TimerEvent&);
    int testBit(int bit, unsigned char *array);
    void initFfDevice();
    void updateFfDevice();

一些成员函数。

2.4 G29ForceFeedback实现

2.4.1 构造函数

G29ForceFeedback::G29ForceFeedback():
    m_device_name("/dev/input/event19"),
    m_Kp(0.1),
    m_Ki(0.0),
    m_Kd(0.0),
    m_offset(0.01),
    m_max_force(1.0),
    m_min_force(0.2),
    m_pub_rate(0.1),
    m_pid_mode(0)
{
    ros::NodeHandle n;
    sub_target = n.subscribe("/ff_target", 1, &G29ForceFeedback::targetCallback, this);

    n.getParam("device_name", m_device_name);
    n.getParam("Kp", m_Kp);
    n.getParam("Ki", m_Ki);
    n.getParam("Kd", m_Kd);
    n.getParam("offset", m_offset);
    n.getParam("max_force", m_max_force);
    n.getParam("min_force", m_min_force);
    n.getParam("pub_rate", m_pub_rate);

    initFfDevice();

    ros::Duration(1).sleep();
    timer = n.createTimer(ros::Duration(m_pub_rate), &G29ForceFeedback::timerCallback, this);
}

逐句分析：

    m_device_name("/dev/input/event19"),

默认使用event19接口

    m_Kp(0.1),
    m_Ki(0.0),
    m_Kd(0.0),
    m_offset(0.01),
    m_max_force(1.0),
    m_min_force(0.2),
    m_pub_rate(0.1),
    m_pid_mode(0)

相关参数设置。

    ros::NodeHandle n;
    sub_target = n.subscribe("/ff_target", 1, &G29ForceFeedback::targetCallback, this);

ros节点初始化，及订阅初始化，订阅topic为“/ff_target”，这个topic就是carla端要发送的topic。

    n.getParam("device_name", m_device_name);
    n.getParam("Kp", m_Kp);
    n.getParam("Ki", m_Ki);
    n.getParam("Kd", m_Kd);
    n.getParam("offset", m_offset);
    n.getParam("max_force", m_max_force);
    n.getParam("min_force", m_min_force);
    n.getParam("pub_rate", m_pub_rate);

参数服务器的设定，直接使用config里面的yaml文件设施，主要是为了方便调参。

    ros::Duration(1).sleep();
    timer = n.createTimer(ros::Duration(m_pub_rate), &G29ForceFeedback::timerCallback, this);

使用createTimer可以创建一个计时器对象，并指定它的触发周期。每次都使用回调函数timerCallback。

2.4.2 timerCallback函数

// update input event with timer callback
void G29ForceFeedback::timerCallback(const ros::TimerEvent&)
{
    updateFfDevice();
}

注意参数是const ros::TimerEvent&，这里有所疑问，以前从未遇见过。大概是形参可以直接省略？

// update input event with writing information to the event file
void G29ForceFeedback::updateFfDevice()
{
    struct input_event event;
    static float diff_i = 0.0, diff = 0.0;
    double diff_d, force, buf;

    // if you wanna use I control, let's avoid integral value exploding
    // static int count = 0;
    // count ++;
    // if (force > 0.3 || count > 10)
    // {
        //     diff_i = 0.0;
        //     count = 0;
        // }

    // calcurate values for PID control
    buf = diff;
    diff = m_target_angle - m_current_angle;
    diff_i += diff;
    diff_d = diff - buf;

    if (m_pid_mode)
    {
        force = fabs(m_Kp * diff + m_Ki * diff_i + m_Kd * diff_d) * ((diff > 0.0) ? 1.0 : -1.0);

        // if wheel angle reached to the target
        if (fabs(diff) < m_offset)
        {
            force = 0.0;
        }
        else
        {
            // force less than 0.2 cannot turn the wheel
            force = (force > 0.0) ? std::max(force, m_min_force) : std::min(force, -m_min_force);
            // set max force for safety
            force = (force > 0.0) ? std::min(force, m_max_force) : std::max(force, -m_max_force);
        }
    }
    else
    {
        force = fabs(m_target_force) * ((diff > 0.0) ? 1.0 : -1.0);

        // if wheel angle reached to the target
        if (fabs(diff) < m_offset)
        {
            force = 0.0;
        }
    }

    // for safety
    force = (force > 0.0) ? std::min(force, m_max_force) : std::max(force, -m_max_force);

    // start effect
    m_effect.u.constant.level = (short)(force * 32767.0);
    m_effect.direction = 0xC000;
    m_effect.u.constant.envelope.attack_level = (short)(force * 32767.0);
    m_effect.u.constant.envelope.fade_level = (short)(force * 32767.0);

    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0)
    {
        std::cout << "failed to upload m_effect" << std::endl;
    }

    // get current state
    while (read(m_device_handle, &event, sizeof(event)) == sizeof(event))
    {
        if (event.type == EV_ABS && event.code == m_axis_code)
        {
            m_current_angle = (event.value - (m_axis_max + m_axis_min) * 0.5) * 2 / (m_axis_max - m_axis_min);
        }
    }
}

逐句分析：

    struct input_event event;
    static float diff_i = 0.0, diff = 0.0;
    double diff_d, force, buf;

input_event 是一个结构体类型，它是 Linux 内核中用于表示输入设备事件的结构体，

定义了两个静态变量diff_i和diff。

diff_d、force和buf是一些参数，是为了使用PID调参，主要是为了调节目标角度和当前角度之间的PID控制。

    // calcurate values for PID control
    buf = diff;
    diff = m_target_angle - m_current_angle;
    diff_i += diff;
    diff_d = diff - buf;

简单描述下pid的过程，

diff是全局变量，其中buf用于存储上一次获得的diff数据，

然后用目标角度减去当前角度来计算出新的diff，

diff_i是diff的积分值，用于累计差值以实现积分控制，

diff_d是微分值，用于计算差值的变化率。

下面是pid控制的过程：

    if (m_pid_mode)
    {
        force = fabs(m_Kp * diff + m_Ki * diff_i + m_Kd * diff_d) * ((diff > 0.0) ? 1.0 : -1.0);

        // if wheel angle reached to the target
        if (fabs(diff) < m_offset)
        {
            force = 0.0;
        }
        else
        {
            // force less than 0.2 cannot turn the wheel
            force = (force > 0.0) ? std::max(force, m_min_force) : std::min(force, -m_min_force);
            // set max force for safety
            force = (force > 0.0) ? std::min(force, m_max_force) : std::max(force, -m_max_force);
        }
    }
    else
    {
        force = fabs(m_target_force) * ((diff > 0.0) ? 1.0 : -1.0);

        // if wheel angle reached to the target
        if (fabs(diff) < m_offset)
        {
            force = 0.0;
        }
    }

其中fabs函数的作用是计算绝对值；

m_Kp在参数服务器下表示的是Kp值；

m_Ki在参数服务器下表示的是Ki值；

m_Kd在参数服务器下表示的是Kd值；

然后分别乘上相应的参数，这行代码的目的是计算出根据PID控制器输出的力或转矩的大小，并且根据diff的正负情况来确定方向。

后面的if和else结构里面是说如果diff小于m_offset的值（设定为0.01）说明到了指定target_angle就将force设置为0。

后面的else就是设定安全范围，在设置的最小和最大值之间。

最后的else是设定不适用Pid控制时，当达到设定的值0.01以下直接置零，其他不操作。

    // start effect
    m_effect.u.constant.level = (short)(force * 32767.0);
    m_effect.direction = 0xC000;
    m_effect.u.constant.envelope.attack_level = (short)(force * 32767.0);
    m_effect.u.constant.envelope.fade_level = (short)(force * 32767.0);

这里用到底了使用ff_effect生成的对象，m_effect。

ff_effect时Linux中使用的力反馈结构，这四行是使用的控制参数，具体含义不太理解。

在网上找到的关于ff_effect的定义：

• type：表示力反馈效果的类型，例如振动、冲击等。
• id：用于标识力反馈效果的唯一 ID。
• direction：表示力的方向，通常使用角度或向量来表示。
• trigger：定义触发力反馈效果的触发器。
• replay：设置力反馈效果的重播次数和时间间隔。
• u：联合体（union），用于存储具体力反馈效果的各种属性和参数。

    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0)
    {
        std::cout << "failed to upload m_effect" << std::endl;
    }

其中ioctl函数是头文件#include <sys/ioctl.h>带入的。

它的作用是将上一步获得的ff_effect对象的相关参数传递给指定的设备，

m_device_handle表示要控制的设备名字，就是可以在参数服务器中设置的，

EVIOCSFF是请求码，表示具体的操作，这里是将m_effect结构体上传到设备，

然后如果返回值小于0表示失败。就会输出"failed to upload m_effect"。

    // get current state
    while (read(m_device_handle, &event, sizeof(event)) == sizeof(event))
    {
        if (event.type == EV_ABS && event.code == m_axis_code)
        {
            m_current_angle = (event.value - (m_axis_max + m_axis_min) * 0.5) * 2 / (m_axis_max - m_axis_min);
        }
    }

使用read函数读取控制设备的状态，并存储在event变量中。

如果event.type == EV_ABS && event.code == m_axis_code符合条件，就将m_current_angle赋值。主要作用是是获取设备上特定轴的当前角度，并将其存储在 m_current_angle变量中。

这里给出在网上找到的关于Linux控制符的相关资料：

链接：Linux输入子系统：事件的编码 -- event-codes.txt_ev_key_DroidPhone的博客-CSDN博客

 Event types:types对应于一个相同逻辑输入结构的一组Codes。每个type都有一组可用的codes用于产生输入事件。每个type可用的codes的详细信息请参考Codes一节的内容。

* EV_SYN:

  - 用于事件间的分割标志。事件可能按时间或空间进行分割，就像在多点触摸协议中的例子。

 * EV_KEY:

  - 用来描述键盘，按键或者类似键盘设备的状态变化。

 * EV_REL:

  - 用来描述相对坐标轴上数值的变化，例如：鼠标向左方移动了5个单位。

 * EV_ABS:

  -用来描述相对坐标轴上数值的变化，例如：描述触摸屏上坐标的值。

 * EV_MSC:

  - 当不能匹配现有的类型时，使用该类型进行描述。

 * EV_SW:

  - 用来描述具备两种状态的输入开关。

 * EV_LED:

  - 用于控制设备上的LED灯的开和关。

 * EV_SND:

  - 用来给设备输出提示声音。

 * EV_REP:

  -用于可以自动重复的设备（autorepeating）。

 * EV_FF:

  - 用来给输入设备发送强制回馈命令。（震动？）

 * EV_PWR:

  - 特别用于电源开关的输入。.

 * EV_FF_STATUS:

  - 用于接收设备的强制反馈状态。

2.4.3 targetCallback函数

// get target information of wheel control from ros message
void G29ForceFeedback::targetCallback(const g29_force_feedback::ForceFeedback &in_target)
{
    m_pid_mode = in_target.pid_mode;
    m_target_angle = in_target.angle;
    m_target_force = in_target.force;
}

使用过ros来接收控制指令，包括是否开启pid、目标角度和控制的力度，其实逻辑是有冲突的。如果选择开启pid，那么force会强制设置为1，这是force就没有用了。

2.4.4 initFfDevice函数

// initialize force feedback device
void G29ForceFeedback::initFfDevice()
{
    // setup device
    unsigned char key_bits[1+KEY_MAX/8/sizeof(unsigned char)];
    unsigned char abs_bits[1+ABS_MAX/8/sizeof(unsigned char)];
    unsigned char ff_bits[1+FF_MAX/8/sizeof(unsigned char)];

    struct input_event event;
    struct input_absinfo abs_info;

    m_device_handle = open(m_device_name.c_str(), O_RDWR|O_NONBLOCK);
    if (m_device_handle < 0)
    {
        std::cout << "ERROR: cannot open device : "<< m_device_name << std::endl;
        exit(1);
    }else{std::cout << "device opened" << std::endl;}

    // which axes has the device?
    memset(abs_bits, 0, sizeof(abs_bits));
    if (ioctl(m_device_handle, EVIOCGBIT(EV_ABS, sizeof(abs_bits)), abs_bits) < 0)
    {
        std::cout << "ERROR: cannot get abs bits" << std::endl;
        exit(1);
    }

    // get some information about force feedback
    memset(ff_bits, 0, sizeof(ff_bits));
    if (ioctl(m_device_handle, EVIOCGBIT(EV_FF, sizeof(ff_bits)), ff_bits) < 0)
    {
        std::cout << "ERROR: cannot get ff bits" << std::endl;
        exit(1);
    }

    // get axis value range
    if (ioctl(m_device_handle, EVIOCGABS(m_axis_code), &abs_info) < 0)
    {
        std::cout << "ERROR: cannot get axis range" << std::endl;
        exit(1);
    }
    m_axis_max = abs_info.maximum;
    m_axis_min = abs_info.minimum;
    if (m_axis_min >= m_axis_max)
    {
        std::cout << "ERROR: axis range has bad value" << std::endl;
        exit(1);
    }

    // check force feedback is supported?
    if(!testBit(FF_CONSTANT, ff_bits))
    {
        std::cout << "ERROR: force feedback is not supported" << std::endl;
        exit(1);
    }else{std::cout << "force feedback supported" << std::endl;}

    // auto centering off
    memset(&event, 0, sizeof(event));
    event.type = EV_FF;
    event.code = FF_AUTOCENTER;
    event.value = 0;
    if (write(m_device_handle, &event, sizeof(event)) != sizeof(event))
    {
        std::cout << "failed to disable auto centering" << std::endl;
        exit(1);
    }

    // initialize constant foce m_effect
    memset(&m_effect, 0, sizeof(m_effect));
    m_effect.type = FF_CONSTANT;
    m_effect.id = -1;
    m_effect.trigger.button = 0;
    m_effect.trigger.interval = 0;
    m_effect.replay.length = 0xffff;
    m_effect.replay.delay = 0;
    m_effect.u.constant.level = 0;
    m_effect.direction = 0xC000;
    m_effect.u.constant.envelope.attack_length = 0;
    m_effect.u.constant.envelope.attack_level = 0;
    m_effect.u.constant.envelope.fade_length = 0;
    m_effect.u.constant.envelope.fade_level = 0;

    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0)
    {
        std::cout << "failed to upload m_effect" << std::endl;
        exit(1);
    }

    // start m_effect
    memset(&event, 0, sizeof(event));
    event.type = EV_FF;
    event.code = m_effect.id;
    event.value = 1;
    if (write(m_device_handle, &event, sizeof(event)) != sizeof(event))
    {
        std::cout << "failed to start event" << std::endl;
        exit(1);
    }
}

初始化函数。

    // setup device
    unsigned char key_bits[1+KEY_MAX/8/sizeof(unsigned char)];
    unsigned char abs_bits[1+ABS_MAX/8/sizeof(unsigned char)];
    unsigned char ff_bits[1+FF_MAX/8/sizeof(unsigned char)];

这是Iinux中相关参数的驱动设置，其中

key_bits数组表示按键的数量，

abs_bits数组表示绝对值事件的数量，

ff_bits数组是设备支持的最大力反馈效果数。

具体见这篇博文：

https://www.cnblogs.com/lifexy/p/7553861.html

    struct input_event event;
    struct input_absinfo abs_info;

两个输入的结构。

    m_device_handle = open(m_device_name.c_str(), O_RDWR|O_NONBLOCK);
    if (m_device_handle < 0)
    {
        std::cout << "ERROR: cannot open device : "<< m_device_name << std::endl;
        exit(1);
    }else{std::cout << "device opened" << std::endl;}

使用open打开函数，打开m_device_name的c语言str类型，并使用O_RDWR|O_NONBLOCK表示打开读写和非堵塞行驶，如果成功打开就m_device_handle大于0，否则小于0。

    // which axes has the device?
    memset(abs_bits, 0, sizeof(abs_bits));
    if (ioctl(m_device_handle, EVIOCGBIT(EV_ABS, sizeof(abs_bits)), abs_bits) < 0)
    {
        std::cout << "ERROR: cannot get abs bits" << std::endl;
        exit(1);
    }

使用memset函数将abs_bits数组全部置为0，

之后使用ioctl函数将获取EV_ABS表示绝对值事件，并使用某种宏将它读取到的东西存放在了abs_bits数组中。如果ioctl小于0就表示示未读取成功。

    // get some information about force feedback
    memset(ff_bits, 0, sizeof(ff_bits));
    if (ioctl(m_device_handle, EVIOCGBIT(EV_FF, sizeof(ff_bits)), ff_bits) < 0)
    {
        std::cout << "ERROR: cannot get ff bits" << std::endl;
        exit(1);
    }

和上面的同理。

    // get axis value range
    if (ioctl(m_device_handle, EVIOCGABS(m_axis_code), &abs_info) < 0)
    {
        std::cout << "ERROR: cannot get axis range" << std::endl;
        exit(1);
    }
    m_axis_max = abs_info.maximum;
    m_axis_min = abs_info.minimum;
    if (m_axis_min >= m_axis_max)
    {
        std::cout << "ERROR: axis range has bad value" << std::endl;
        exit(1);
    }

判断能否获得轴的信息。

    // check force feedback is supported?
    if(!testBit(FF_CONSTANT, ff_bits))
    {
        std::cout << "ERROR: force feedback is not supported" << std::endl;
        exit(1);
    }else{std::cout << "force feedback supported" << std::endl;}

测试是否支持力反馈。

    // auto centering off
    memset(&event, 0, sizeof(event));
    event.type = EV_FF;
    event.code = FF_AUTOCENTER;
    event.value = 0;
    if (write(m_device_handle, &event, sizeof(event)) != sizeof(event))
    {
        std::cout << "failed to disable auto centering" << std::endl;
        exit(1);
    }

将event初始化置为0，

并将相关参数初始化，

EV_FF表示是力反馈，

FF_AUTOCENTER表示是自动居中控制的代码，

value将它置零就是关闭这个功能。

    // initialize constant foce m_effect
    memset(&m_effect, 0, sizeof(m_effect));
    m_effect.type = FF_CONSTANT;
    m_effect.id = -1;
    m_effect.trigger.button = 0;
    m_effect.trigger.interval = 0;
    m_effect.replay.length = 0xffff;
    m_effect.replay.delay = 0;
    m_effect.u.constant.level = 0;
    m_effect.direction = 0xC000;
    m_effect.u.constant.envelope.attack_length = 0;
    m_effect.u.constant.envelope.attack_level = 0;
    m_effect.u.constant.envelope.fade_length = 0;
    m_effect.u.constant.envelope.fade_level = 0;

一些参数设置，应该是linux中设定力反馈的参数。

    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0)
    {
        std::cout << "failed to upload m_effect" << std::endl;
        exit(1);
    }

    // start m_effect
    memset(&event, 0, sizeof(event));
    event.type = EV_FF;
    event.code = m_effect.id;
    event.value = 1;
    if (write(m_device_handle, &event, sizeof(event)) != sizeof(event))
    {
        std::cout << "failed to start event" << std::endl;
        exit(1);
    }

开始力反馈。

2.4.5 testBit函数

int G29ForceFeedback::testBit(int bit, unsigned char *array)
{
    return ((array[bit / (sizeof(unsigned char) * 8)] >> (bit % (sizeof(unsigned char) * 8))) & 1);
}

三、总结

对于Linux中相关指令的调用需要很熟悉，特别是输入输出相关的内容。文章来源地址https://www.toymoban.com/news/detail-621835.html

到了这里，关于carla与g29联合调试(二)的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！