前言：

之前做的carla与g29的联合调试，现在记录一下carla控制g29的实现流程。

一、总体通讯流程

主要实现为carla中车辆的方向盘转动带着g29跟着一起转动，使用ros通讯来实现这个过程。

二、具体实现流程

2.1首先确定g29的力反馈和转动

使用的是开源的方案：g29力反馈

$ cd catkin_ws/src 
$ git clone https://github.com/ncnynl/ros-g29-force-feedback.git
$ cd ../ 
$ catkin_make

编译通过后。查看打开CMakeLists.txt文件，这里的：

这里就是向g29发送控制指令的node，使用前必须现运行这个node。在运行前，需要确定g29插入usb的端口。

使用命令：

cat /proc/bus/input/devices

查看g29的端口：

现在获取它的端口为event4 js0，分别是力反馈和方向盘转动的端口。

然后修改yaml文件，一般在config文件夹里面：

没有的话，自己创建一个。

然后修改这里的device_name这里的部分。

然后将这个config写入，使用命令：

rosparam load ./src/ros-g29-force-feedback/config/g29.yaml 
rosrun g29_force_feedback g29_force_feedback_node

出现下面的标志就表示g29的旋转与力反馈接口成功打开。

现在测试一下，使用命令：

source devel/setup.bash
rosrun g29_force_feedback talker.py

然后可以在talker.py里面修改相关的参数，g29msg.angle是转动的角度，g29msg.force是力反馈的力度。

#!/usr/bin/env python                                                                                                                 
import rospy

from std_msgs.msg import String
from g29_force_feedback.msg import ForceFeedback

def talker():
    rospy.init_node('talker')
    pub = rospy.Publisher('ff_target', ForceFeedback, queue_size=20)
#    pub = rospy.Publisher('g29test', String, queue_size=20)                                                                          

    r = rospy.Rate(0.5)
    while not rospy.is_shutdown():
      # str = "angle: 0.5 force: 0.6"                                                                                                 
      g29msg = ForceFeedback()
      g29msg.angle = 0
      g29msg.force = 0.4
      rospy.loginfo(g29msg)
      pub.publish(g29msg)
      #break                                                                                                                          
      r.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException: pass

会输出信息：

同时方向盘有转动，表示力反馈设置成功。这里的talker.py写的很清晰，就不解析了，之后也是在这个框架上简单的修改后达成carla中车辆控制g29方向盘的效果。

2.2 输出carla中车辆的转动信息

依据前面实现力反馈的包，使用ros来输出carla中车辆转动信息。需要自己根据carla中的车辆转动信息来自定义设置消息格式。我没有使用carla_ros_bridge自己写了单独的接口，具体实现参考前面写的文章：

carla和ros不通过carla_ros_bridge进行lidar发送_hex_refugeeeee的博客-CSDN博客

这里给出实现的源码：

# 将车辆设置成自动驾驶模式

import rospy
from pub_steer.msg import Carla_steer
import carla
import random
from agents.navigation.behavior_agent import BehaviorAgent

def get_steering_wheel_angle(vehicle):
    # 获取车辆的方向盘信息
    control = vehicle.get_control()
    steering_wheel_angle = control.steer

    return steering_wheel_angle

def main():
    client = carla.Client('localhost', 2000)
    client.set_timeout(10.0)
    world = client.get_world()
    origin_settings = world.get_settings()
    settings = world.get_settings()
    settings.synchronous_mode = True
    settings.fixed_delta_seconds = 0.05
    world.apply_settings(settings)

    blueprint_library = world.get_blueprint_library()

    try:
        # 确定起点和终点
        p11 = carla.Location(229, 116, 2)
        p12 = carla.Location(240, 116, 2)
        p21 = carla.Location(20, 194, 2)
        p22 = carla.Location(20, 198, 2)
        start_point1 = carla.Transform(p11, carla.Rotation(0,90,0))

        end_point1 = carla.Transform(p21, carla.Rotation(0, 0, 0))

        
        # 创建车辆
        ego_vehicle_bp = blueprint_library.find('vehicle.audi.a2')
        ego_vehicle_bp.set_attribute('color', '0, 0, 0')

        # ROS节点初始化
        rospy.init_node('carla_steer_publisher', anonymous=True)
        car_info_pub = rospy.Publisher('vehicle_steer_info', Carla_steer, queue_size=10)

        # 两种模式，设置成自动驾驶模式
        spawn_points = world.get_map().get_spawn_points()
        random.shuffle(spawn_points)

        vehicle1 = world.spawn_actor(ego_vehicle_bp, spawn_points[3])
        # 设置车辆的驾驶模式
        agent1 = BehaviorAgent(vehicle1, behavior='normal')
        if spawn_points[0].location != agent1._vehicle.get_location():
            destination = spawn_points[0]
        else:
            destination = spawn_points[1]

        agent1.set_destination(end_point1.location)

        while not rospy.is_shutdown():
            world.tick()
            agent1._update_information()
            if (len(agent1._local_planner._waypoints_queue) < 1):
                random.shuffle(spawn_points)
                if spawn_points[0].location != agent1._vehicle.get_location():
                    destination = spawn_points[0]
                else:
                    destination = spawn_points[1]
                agent1.set_destination(destination.location)
            # 设置速度限制
            speed_limit1 = vehicle1.get_speed_limit()
            agent1.get_local_planner().set_speed(speed_limit1)
 
            control1 = agent1.run_step(debug=True)
            vehicle1.apply_control(control1)

            # 获得carla中车辆的方向盘传动信息 steering_angle
            steering_angle = get_steering_wheel_angle(vehicle1)
            
            car_info_msg = Carla_steer()
            car_info_msg.steer_angle = steering_angle
            print("方向盘转动：", steering_angle)
            car_info_pub.publish(car_info_msg)
    
    finally:
        world.apply_settings(origin_settings)
        vehicle1.destroy()

if __name__ == '__main__':

    try:
        main()
    except KeyboardInterrupt:
        pass
    finally:
        print('\ndone.')

注释写的很清晰，注意使用了自定义消息类型，我写的自定义消息类型是这个，可以根据不同的需要自己写：

主要实现就是这些，其他的部分比较简单。

这节就将carla中车辆的转向消息通过ros自定义消息类型发送出去了，整个项目实现了2/3了。

2.3 接收carla发送的方向盘转动信息

这节就是前面提到的，根据talker.py的框架写的接收上一节中发送的方向盘转动信息。比较简单，直接给出源码。

#!/usr/bin/env python                                                                                                                 
import rospy

from g29_force_feedback.msg import Carla_steer
from g29_force_feedback.msg import ForceFeedback    


pub = rospy.Publisher('ff_target', ForceFeedback, queue_size=20)

def sub_g29(data):
    steer_datas = data.steer_angle
    print(steer_datas)    
    g29msg = ForceFeedback()
    g29msg.angle = steer_datas
    g29msg.force = 0.4
    # rospy.loginfo(g29msg)
    pub.publish(g29msg)

def talker():
    rospy.init_node('talker')

    rospy.Subscriber('vehicle_steer_info', Carla_steer, sub_g29)

    rospy.spin()                                                            

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException: pass

到这里基本上所有的实现就完成了，rqt：