一、トピック通信

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;
    // 効果のアップロード
    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0) {
        std::cout << "効果のアップロードに失敗しました" << 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) {

    // 効果の設定
    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;

    // 効果のアップロード
    if (ioctl(m_device_handle, EVIOCSFF, &m_effect) < 0) {
        std::cout << "効果のアップロードに失敗しました" << 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 << "エラー: デバイスを開けません: "<< m_device_name << std::endl;
        exit(1);

    } else {std::cout << "デバイスが開かれました" << 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 << "エラー: absビットを取得できません" << 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 << "エラー: ffビットを取得できません" << std::endl;
        exit(1);
    }

    // 軸値範囲の取得
    if (ioctl(m_device_handle, EVIOCGABS(m_axis_code), &abs_info) < 0) {
        std::cout << "エラー: 軸範囲を取得できません" << 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 << "エラー: 軸範囲に不正な値があります" << std::endl;
        exit(1);
    }

    // 力フィードバックのサポートを確認
    if(!testBit(FF_CONSTANT, ff_bits)) {
        std::cout << "エラー: 力フィードバックはサポートされていません" << std::endl;
        exit(1);

    } else { std::cout << "力フィードバックがサポートされています" << 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 << "自動中央復帰の無効化に失敗しました" << std::endl;
        exit(1);
    }

    // 初期化
    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; // 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 << "効果のアップロードに失敗しました" << 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 << "イベントの開始に失敗しました" << 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

    # Carlaサーバーに接続
    client = carla.Client('localhost', 2000)
    client.set_timeout(2.0)
    world = client.get_world()

    # マップのスポーンポイントを取得
    spawn_points = world.get_map().get_spawn_points()

    # 車両のブループリントを作成
    blueprint_library = world.get_blueprint_library()
    vehicle_bp = blueprint_library.filter('model3')[0]

    # 車両をスポーン
    vehicle = world.spawn_actor(vehicle_bp, random.choice(spawn_points))

    # オートパイロットを20 m/sで設定
    vehicle.set_autopilot(True)

    # スペクテーター（カメラ）を設定
    spectator = world.get_spectator()


    # メインループ
    try:
        while not rospy.is_shutdown():
            # スペクテーター位置を車両に追従させる
            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)
            # ステアリング角度を計算
            steering_angle = vehicle.get_control().steer

            # メッセージを作成して公開
            msg = ForceFeedback()
            msg.header.stamp = rospy.Time.now()
            msg.position = steering_angle
            msg.torque = 0.8

            pub.publish(msg)
            rospy.loginfo(f"公開: 角度={msg.position}, 力={msg.torque}")

            rate.sleep()

    except rospy.ROSInterruptException:
        pass
    finally:
        # 車両を停止して破棄
        vehicle.set_autopilot(False)
        vehicle.destroy()


if __name__ == '__main__':
    main()

実装効果は以下の通りです： Carla-G29力フィードバック_哔哩哔哩_bilibili

G29

これらにはROSとCarlaの基本的な知識が必要です。