如何通过rostopic发布速度命令控制AirSim中的车辆

正如我在前文中所说：使用AirSim进行仿真时，在Car的SimMode下，可以用键盘控制车辆行进，但是一旦开启ROS接口，ros node将会全面接管仿真车辆的输入输出，这时就不能再用键盘控制车辆了。我们可以用rospy再编写一个ros节点来通过键盘以发送rostopic的形式发送速度控制命令给ROS接口。

参考ros自带的键盘控制节点：ros_teleop

思路也非常简单，把里边使用的geometry_msgs.msg下的Twist数据格式换成airsim自带的CarControls message就好了。

根据ros相关知识（在此仅限ROS1）我们知道，想要发布自编格式的rostopic信息，发布信息的ros package的package.xml里要写入包含topic message定义的package，例如在package.xml里写下：

<build_depend>airsim_ros_pkgs</build_depend>
<exec_depend>airsim_ros_pkgs</exec_depend>

因为CarControls这个message的.msg文件在：AirSim/ros/src/airsim_ros_pkgs/msg里。

CarControls的定义如下：

std_msgs/Header header
float32 throttle
float32 brake
float32 steering
bool handbrake
bool manual
int8 manual_gear
bool gear_immediate

接下来写python脚本，以下是脚本内容（ros kinetic所限，是python2）

import rospy
# from geometry_msgs.msg import Twist
# 对比于ros_teleop的这一句import Twist，在这里我们import AirSim的控制命令类型CarControls：
from airsim_ros_pkgs.msg import CarControls
import sys, select, termios, tty

# 在这里设置一下线速度和角速度最大值，以及线速度角速度的步进量：
MAX_LIN_VEL = 1.0
MAX_ANG_VEL = 1.0

LIN_VEL_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.1

msg = """
Control the Airsim Car. 
---------------------------
Moving around:
        w
   a    s    d
        x

w/x : increase/decrease linear velocity ( ~ 1.0)
a/d : increase/decrease angular velocity ( ~ 1.0)

space key, s : force stop

CTRL-C to quit
"""

e = """
Communications Failed
"""

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

def vels(target_linear_vel, target_angular_vel):
    return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)

def makeSimpleProfile(output, input, slop):
    if input > output:
        output = min( input, output + slop )
    elif input < output:
        output = max( input, output - slop )
    else:
        output = input

    return output

def constrain(input, low, high):
    if input < low:
      input = low
    elif input > high:
      input = high
    else:
      input = input

    return input

def checkLinearLimitVelocity(vel):
    vel = constrain(vel, -MAX_LIN_VEL, MAX_LIN_VEL)
    return vel

def checkAngularLimitVelocity(vel):  
    vel = constrain(vel, -MAX_ANG_VEL, MAX_ANG_VEL)   
    return vel

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('airsim_car_teleop_joy')
    # 设置一下要发布的rostopic的名称和数据格式：
    pub = rospy.Publisher('car_cmd', CarControls, queue_size=10)

    status = 0
    target_linear_vel   = 0.0
    target_angular_vel  = 0.0
    control_linear_vel  = 0.0
    control_angular_vel = 0.0

    try:
        print msg
        while(1):
            key = getKey()
            if key == 'w' :
                target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE)
                status = status + 1
                print vels(target_linear_vel,target_angular_vel)
            elif key == 'x' :
                target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE)
                status = status + 1
                print vels(target_linear_vel,target_angular_vel)
            elif key == 'a' :
                target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
                status = status + 1
                print vels(target_linear_vel,target_angular_vel)
            elif key == 'd' :
                target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
                status = status + 1
                print vels(target_linear_vel,target_angular_vel)
            elif key == ' ' or key == 's' :
                target_linear_vel   = 0.0
                control_linear_vel  = 0.0
                target_angular_vel  = 0.0
                control_angular_vel = 0.0
                print vels(target_linear_vel, target_angular_vel)
            else:
                if (key == '\x03'):
                    break

            if status == 20 :
                print msg
                status = 0

            # twist = Twist()
            # vel = VelCmd()
            # 实例化一个发布的信息的类：
            car_commands = CarControls()
            car_commands.gear_immediate = True

            control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))

            control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
			
			# 接下来就是重点，键盘上的awsd需要翻译成汽车需要的控制命令：
            if control_linear_vel==0.0:
            	# 如果线速度为0，刹车：
                car_commands.handbrake = True
                car_commands.brake = 1
            elif control_linear_vel>0:
            	# 如果线速度大于0，先松刹车
                car_commands.handbrake = False
                car_commands.brake = 0
                # 挂挡：
                car_commands.manual = False
                car_commands.manual_gear = 0
                # 设置速度：
                car_commands.throttle = control_linear_vel
                # 这里加负号，才能让a对应左拐，d对应右拐：
                car_commands.steering = -control_angular_vel
            else:
                car_commands.handbrake = False
                car_commands.brake = 0
                # 线速度小于0，挂倒挡：
                car_commands.manual = True
                car_commands.manual_gear = -1
                car_commands.throttle = control_linear_vel
                car_commands.steering = -control_angular_vel
			
			# 把控制命令当做rostopic发布出去：
            pub.publish(car_commands)

    except:
        print e

    finally:
        # twist = Twist()
        # 结束节点之后刹车：
        car_commands = CarControls()
        car_commands.handbrake = True
        car_commands.brake = 1
        pub.publish(car_commands)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

以下是运行此节点，用键盘控制AirSim中的车辆的截图：

程序文件保存在：

github链接