前言：
对于力反馈的源码解析。
一、工作空间分析
工作空间如下图所示：
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中相关指令的调用需要很熟悉，特别是输入输出相关的内容。
4240
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
