Ubuntu20.04下Carla-G29力反馈控制

一、话题通信

Carla和G29通过ROS话题进行通信，消息是自定义的：

std_msgs/Header header
float32 position
float32 torque

二、G29力反馈控制

之前看了很多教程，用Carla控制G29时，力反馈的力不好设定，会出现方向盘震荡或者无法回中的问题，针对这些问题添加计算旋转力和居中力的函数，具体为：

1、计算旋转力（calcRotateForce）：如果差异很小（小于m_eps），则不施加力；如果差异在刹车范围内，施加一个相反方向的较小力矩；否则，施加目标力矩，方向取决于差异的正负。实现代码：

void G29ForceFeedback::calcRotateForce(double &torque,
                                       double &attack_length,
                                       const ros_g29_force_feedback::ForceFeedback &target,
                                       const double &current_position) {
    double diff = target.position - current_position;
    double direction = (diff > 0.0) ? 1.0 : -1.0;

    if (fabs(diff) < m_eps) {
        torque = 0.0;
        attack_length = 0.0;
    } else if (fabs(diff) < m_brake_position) {
        m_is_brake_range = true;
        torque = target.torque * m_brake_torque_rate * -direction;
        attack_length = m_loop_rate;
    } else {
        torque = target.torque * direction;
        attack_length = m_loop_rate;
    }
}

 2、计算居中力（calcCenteringForce）：力矩的方向总是指向中心位置，如果差异很小，不施加力。否则，根据差异的大小计算一个力矩，力矩随着差异增大而增大，但有上限。

void G29ForceFeedback::calcCenteringForce(double &torque,
                                          const ros_g29_force_feedback::ForceFeedback &target,
                                          const double &current_position) {
    double diff = target.position - current_position;
    double direction = (diff > 0.0) ? 1.0 : -1.0;

    if (fabs(diff) < m_eps)
        torque = 0.0;
    else {
        double torque_range = m_auto_centering_max_torque - m_min_torque;
        double power = (fabs(diff) - m_eps) / (m_auto_centering_max_position - m_eps);
        double buf_torque = power * torque_range + m_min_torque;
        torque = std::min(buf_torque, m_auto_centering_max_torque) * direction;
    }
}

完整的力反馈代码如下，相关解释在代码中备注了，这里不进行解释：

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

#include "ros_g29_force_feedback/ForceFeedback.h"

class G29ForceFeedback {

private:
    ros::Subscriber sub_target;
    ros::Timer timer;

    int m_device_handle;
    int m_axis_code = ABS_X;
    int m_axis_min;
    int m_axis_max;

    // 设备参数
    std::string m_device_name = "/dev/input/event11";
    double m_loop_rate = 0.1;
    double m_max_torque = 1.0;
    double m_min_torque = 0.2;
    double m_brake_position = 0.2;
    double m_brake_torque_rate = 0.1;
    double m_auto_centering_max_torque = 0.3;
    double m_auto_centering_max_position = 0.2;
    double m_eps = 0.01;
    bool m_auto_centering = false;


    ros_g29_force_feedback::ForceFeedback m_target;
    bool m_is_target_updated = false;
    bool m_is_brake_range = false;
    struct ff_effect m_effect;
    double m_position;
    double m_torque;
    double m_attack_length;

public:
    G29ForceFeedback();
    ~G29ForceFeedback();

private:
    void targetCallback(const ros_g29_force_feedback::ForceFeedback &in_target);
    void loop(const ros::TimerEvent&);
    int testBit(int bit, unsigned char *array);
    void initDevice();
    void calcRotateForce(double &torque, double &attack_length, const ros_g29_force_feedback::ForceFeedback &target, const double &current_position);
    void calcCenteringForce(double &torque, const ros_g29_force_feedback::ForceFeedback &target, const double &current_position);
    void uploadForce(const double &position, const double &force, const double &attack_length);
};

// 构造函数
G29ForceFeedback::G29ForceFeedback() {

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

    n.getParam("device_name", m_device_name);
    n.getParam("loop_rate", m_loop_rate);
    n.getParam("max_torque", m_max_torque);
    n.getParam("min_torque", m_min_torque);
    n.getParam("brake_position", m_brake_position);
    n.getParam("brake_torque_rate", m_brake_torque_rate);
    n.getParam("auto_centering_max_torque", m_auto_centering_max_torque);
    n.getParam("auto_centering_max_position", m_auto_centering_max_position);
    n.getParam("eps", m_eps);
    n.getParam("auto_centering", m_auto_centering);

    initDevice();

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

//  重置力反馈效果
G29ForceFeedback::~G29ForceFeedback() {

    m_effect.type = FF_CONSTANT;
    m_effect.id = -1;
    m_effect.u.constant.level = 0;
    m_effect.direction = 0;
    // upload m_effect
    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0) {
        std::cout << "failed to upload m_effect" << std::endl;
    }
}


// 读取当前方向盘位置，计算并应用力反馈
void G29ForceFeedback::loop(const ros::TimerEvent&) {

    struct input_event event;
    double last_position = m_position;
    // 获取当前状态
    while (read(m_device_handle, &event, sizeof(event)) == sizeof(event)) {
        if (event.type == EV_ABS && event.code == m_axis_code) {
            m_position = (event.value - (m_axis_max + m_axis_min) * 0.5) * 2 / (m_axis_max - m_axis_min);
        }
    }

    if (m_is_brake_range || m_auto_centering) {
        calcCenteringForce(m_torque, m_target, m_position);
        m_attack_length = 0.0;

    } else {
        calcRotateForce(m_torque, m_attack_length, m_target, m_position);
        m_is_target_updated = false;
    }

    uploadForce(m_target.position, m_torque, m_attack_length);
}

// 计算旋转力
void G29ForceFeedback::calcRotateForce(double &torque,
                                       double &attack_length,
                                       const ros_g29_force_feedback::ForceFeedback &target,
                                       const double &current_position) {

    double diff = target.position - current_position;
    double direction = (diff > 0.0) ? 1.0 : -1.0;

    if (fabs(diff) < m_eps) {
        torque = 0.0;
        attack_length = 0.0;

    } else if (fabs(diff) < m_brake_position) {
        m_is_brake_range = true;
        torque = target.torque * m_brake_torque_rate * -direction;
        attack_length = m_loop_rate;

    } else {
        torque = target.torque * direction;
        attack_length = m_loop_rate;
    }
}

// 计算居中力
void G29ForceFeedback::calcCenteringForce(double &torque,
                                          const ros_g29_force_feedback::ForceFeedback &target,
                                          const double &current_position) {

    double diff = target.position - current_position;
    double direction = (diff > 0.0) ? 1.0 : -1.0;

    if (fabs(diff) < m_eps)
        torque = 0.0;

    else {
        double torque_range = m_auto_centering_max_torque - m_min_torque;
        double power = (fabs(diff) - m_eps) / (m_auto_centering_max_position - m_eps);
        double buf_torque = power * torque_range + m_min_torque;
        torque = std::min(buf_torque, m_auto_centering_max_torque) * direction;
    }
}


//将计算的力应用到G29
void G29ForceFeedback::uploadForce(const double &position,
                                   const double &torque,
                                   const double &attack_length) {

    // std::cout << torque << std::endl;
    // set effect
    m_effect.u.constant.level = 0x7fff * std::min(torque, m_max_torque);
    m_effect.direction = 0xC000;
    m_effect.u.constant.envelope.attack_level = 0; /* 0x7fff * force / 2 */
    m_effect.u.constant.envelope.attack_length = attack_length;
    m_effect.u.constant.envelope.fade_level = 0;
    m_effect.u.constant.envelope.fade_length = attack_length;

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


//接收订阅的消息
void G29ForceFeedback::targetCallback(const ros_g29_force_feedback::ForceFeedback &in_msg) {

    if (m_target.position == in_msg.position && m_target.torque == fabs(in_msg.torque)) {
        m_is_target_updated = false;

    } else {
        m_target = in_msg;
        m_target.torque = fabs(m_target.torque);
        m_is_target_updated = true;
        m_is_brake_range = false;
    }
}


//初始化
void G29ForceFeedback::initDevice() {
    // 设备设置
    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;}

    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(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);
    }

    // 获取轴值范围
    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);
    }

    // 检查是否支持力反馈
    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; }

    // 自动对中
    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);
    }

    // 初始化
    memset(&m_effect, 0, sizeof(m_effect));
    m_effect.type = FF_CONSTANT;
    m_effect.id = -1; // initial value
    m_effect.trigger.button = 0;
    m_effect.trigger.interval = 0;
    m_effect.replay.length = 0xffff;  // longest value
    m_effect.replay.delay = 0; // delay from write(...)
    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);
    }

    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);
    }
}


int G29ForceFeedback::testBit(int bit, unsigned char *array) {

    return ((array[bit / (sizeof(unsigned char) * 8)] >> (bit % (sizeof(unsigned char) * 8))) & 1);
}


int main(int argc, char **argv ){

    ros::init(argc, argv, "ros_g29_force_feedback_node");
    G29ForceFeedback g29_ff;
    ros::spin();
    return(0);
}

运行代码前需要更改设备参数，在终端输入 cat /proc/bus/input/devices查看G29对应的event后面的数字，并修改代码相应部分：

std::string m_device_name = "/dev/input/event11";

三、Carla获取方向盘信息

#!/usr/bin/env python

import rospy
import carla
import random
import math
import time
from ros_g29_force_feedback.msg import ForceFeedback


def main():
    rospy.init_node('carla_steering_publisher', anonymous=True)
    pub = rospy.Publisher('ff_target', ForceFeedback, queue_size=10)
    rate = rospy.Rate(10) # 10hz

    # Connect to Carla server
    client = carla.Client('localhost', 2000)
    client.set_timeout(2.0)
    world = client.get_world()

    # Get the map's spawn points
    spawn_points = world.get_map().get_spawn_points()

    # Create a blueprint for the vehicle
    blueprint_library = world.get_blueprint_library()
    vehicle_bp = blueprint_library.filter('model3')[0]

    # Spawn the vehicle
    vehicle = world.spawn_actor(vehicle_bp, random.choice(spawn_points))

    # Set up the autopilot with a speed of 20 m/s
    vehicle.set_autopilot(True)

    # Set up the spectator (camera)
    spectator = world.get_spectator()


    # Main loop
    try:
        while not rospy.is_shutdown():
            # Update spectator position to follow the vehicle
            vehicle_transform = vehicle.get_transform()
            bv_transform = carla.Transform(vehicle_transform.location + carla.Location(z=25,x=0), carla.Rotation(yaw=0, pitch=-90))
            spectator.set_transform(bv_transform)
            # Calculate steering angle
            steering_angle = vehicle.get_control().steer

            # Create and publish the message
            msg = ForceFeedback()
            msg.header.stamp = rospy.Time.now()
            msg.position = steering_angle
            msg.torque = 0.8

            pub.publish(msg)
            rospy.loginfo(f"Published: angle={msg.position}, force={msg.torque}")

            rate.sleep()

    except rospy.ROSInterruptException:
        pass
    finally:
        # Stop the vehicle and destroy it
        vehicle.set_autopilot(False)
        vehicle.destroy()


if __name__ == '__main__':
    main()

实现效果如下：
Carla-G29力反馈_哔哩哔哩_bilibili

G29

这些需要对ROS和Carla有一定的熟悉程度。