ROS中Twist和AckermannDriveStamped消息类型的相互转换

滴滴哒哒答答

已于 2025-04-15 16:03:04 修改

阅读量794

点赞数 10

分类专栏： ROS 文章标签： python 开发语言

于 2024-11-06 16:29:09 首次发布

本文链接：https://blog.csdn.net/m0_49384824/article/details/143569973

版权

ROS 专栏收录该内容

2 篇文章

订阅专栏

在 ROS 中的阿克曼模型机器人里，geometry_msgs/Twist 消息通常定义在车辆后桥的中心。这是因为阿克曼转向车辆的运动控制通常围绕后桥中心进行，尤其是速度和角速度的计算。

ROS 中运动学分析为正解（Forward kinematics）和逆解（Inverse Kinematics）两种。

正解或正运动学模型（forward kinematic model）：是将获得的机器人底盘速度指令 /cmd_vel 转化为每个车轮的实际速度。
逆解或逆运动学模型（inverse kinematic model）：是根据电机编码器获得的每个车轮速度计算出机器人底盘速度，从而实现航迹推算

一、两种消息类型的介绍

消息geometry_msgs/Twist 消息类型

# geometry_msgs/Twist 消息类型
Vector3 linear    # 虚拟的中间轮胎的线速度 (m/s)
Vector3 angular   # 虚拟的中间轮胎的角速度 (rad/s)

ackermann_msgs/AckermannDriveStamped 消息类型

# ackermann_msgs/AckermannDriveStamped 消息类型
# 其中 steering_angle 和 speed 是必需的

float32 steering_angle          # 虚拟的中间轮胎的转角 (rad)
float32 steering_angle_velocity # 虚拟的中间轮胎的角速度 (rad/s)

float32 speed                   # 虚拟的中间轮胎的线速度 (m/s)
float32 acceleration            # 虚拟的中间轮胎的加速度 (m/s^2)
float32 jerk                    # 虚拟的中间轮胎的冲击度 (m/s^3)

二、四轮阿克曼正运动学模型

四轮阿克曼模型如下， $L$ 为轴距， $B_f$ 为前轮轮距， $B_r$ 为后轮轮距

根据上图，定义车辆绕旋转中心 $O$ 的旋转角速度为 $w$ ，各轮车速满足：

$\omega=\frac{\nu}{R}=\frac{\nu_{1}}{R_{1}}=\frac{\nu_{2}}{R_{2}}=\frac{\nu_{3}}{R_{3}}=\frac{\nu_{4}}{R_{4}}$

已知 geometry_msgs/Twist 消息中的 $v$ 和 $w$ ，可求得 虚拟的中间轮胎的转向半径 $R$ 和 前轮转角 $\delta$ ，也就是图中的 $R$ 和 $\delta$ 。其中 $v=Twist.linear.x$ ， $w=Twist.angular.z$ 。

$R=\frac{\nu}{\omega}$

$\tan\delta=\frac{L}{R}=\frac{wL}{v}$

四个轮的速度 $v_i$ 和各轮转弯半径 $R_i$ 正比，即：

$\nu_{1}=\nu\frac{R_1}{R}, R_1=\sqrt{L^{2}+(R-\frac{B_f}{2})^{2}}$

$\nu_{2}=\nu\frac{R_2}{R}, R_2=\sqrt{L^{2}+(R+\frac{B_f}{2})^{2}}$

$\nu_{4}=\nu\frac{R_4}{R}, R_4=R-\frac{B_r}{2}$

$\nu_{3}=\nu\frac{R_3}{R}, R_3=R+\frac{B_r}{2}$

带入得四个轮的速度 $v_i$ 为：

$\nu_{1}=\nu\sqrt{\frac{L^{2}}{R^{2}}+\left(1-\frac{B_{\mathrm{f}}}{2R}\right)^{2}}=\nu\sqrt{\mathrm{tan}\delta^{2}+\left(1-\frac{B_{\mathrm{f}}\mathrm{tan}\delta}{2L}\right)^{2}}$

$\nu_{2}=\nu\sqrt{\frac{L^{2}}{R^{2}}+\left(1+\frac{B_{\mathrm{f}}}{2R}\right)^{2}}=\nu\sqrt{\mathrm{tan}\delta^{2}+\left(1+\frac{B_{\mathrm{f}}\mathrm{tan}\delta}{2L}\right)^{2}}$

$\nu_{4}=\nu\left(\begin{matrix}{1-\frac{B_{r}}{2R}}\\\end{matrix}\right)=\nu\left(\begin{matrix}{1-\frac{B_{r}\mathbf{tan\delta}}{2L}}\\\end{matrix}\right)$

$\nu_{3}=\nu\left(\begin{matrix}{1+\frac{B_{r}}{2R}}\\\end{matrix}\right)=\nu\left(\begin{matrix}{1+\frac{B_{r}\mathbf{tan\delta}}{2L}}\\\end{matrix}\right)$

前轮的转角 $\delta_1$ 和 $\delta_2$ 为：

$\tan\delta_1=\frac{L}{R-\frac{B_f}{2}}$

$\tan\delta_2=\frac{L}{R+\frac{B_f}{2}}$

至此，四轮阿克曼模型的四轮速度 $v_1,v_2,v_3,v_4$ 和两个前轮的转角 $\delta_1,\delta_2$ 均已求出。

三、两种消息类型的转换

geometry_msgs/Twist -》ackermann_msgs/AckermannDriveStamped

根据 $R=\frac{\nu}{\omega}$ 计算得到虚拟的中间轮胎的转向半径 $R$ 。其中 $v=Twist.linear.x$ ， $w=Twist.angular.z$ 。
根据 $\tan\delta=\frac{L}{R}=\frac{wL}{v}$ 计算得到虚拟的中间轮胎的前轮转角 $\delta$
将 $\delta$ 和原始的Twist消息中的 $v$ 分别赋值给 AckermannDriveStamped 消息中的 steering_angle 和 speed。

四、代码

#!/usr/bin/env python3
# -*- coding: utf-8 -*- 
import rospy
import math
from std_msgs.msg import Bool
from std_msgs.msg import Float32
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
from ackermann_msgs.msg import AckermannDriveStamped


L = 0.335 # 轴距，单位 m
B_f = 0.305 # 前轮轮距，单位 m
B_r = 0.305 # 后轮轮距，单位 m
max_rad = 0.7 # 最大转弯角度，单位 rad


# 设置最大的角度
def limsteer(data, maxdata):
    if data > 0 and data > maxdata:
        data = maxdata
    elif data < 0 and math.fabs(data) > maxdata:
        data = maxdata
    return data


# 接受 ackermann_msgs/AckermannDriveStamped 类型的消息 控制 阿克曼小车模型四个轮子的速度和前轮转角
def set_throttle_steer(data):
    global L, B_f, B_r, max_rad

    pub_vel_left_rear_wheel = rospy.Publisher('/car/left_rear_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_right_rear_wheel = rospy.Publisher('/car/right_rear_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_left_front_wheel = rospy.Publisher('/car/left_front_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_right_front_wheel = rospy.Publisher('/car/right_front_wheel_velocity_controller/command', Float64, queue_size=1)

    pub_pos_left_steering_hinge = rospy.Publisher('/car/left_steering_hinge_position_controller/command', Float64, queue_size=1)
    pub_pos_right_steering_hinge = rospy.Publisher('/car/right_steering_hinge_position_controller/command', Float64, queue_size=1)

    v = data.drive.speed                       # 速度 (m/s)
    rad = data.drive.steering_angle            # 角度 (rad)

    if v != 0 and rad != 0:
        # r 为理想的中间轮胎的转弯半径
        r = L / math.tan(rad)
        w = v / r

        # math.copysign(x, y)：返回一个与 x 数值大小相同、与 y 符号相同的浮点数
        rL_rear = r - (math.copysign(1, w) * (B_r/2.0)) 
        rR_rear = r + (math.copysign(1, w) * (B_r/2.0))

        L_front_temp = r - (math.copysign(1, w) * (B_f/2.0)) 
        R_front_temp = r + (math.copysign(1, w) * (B_f/2.0))
        rL_front = math.sqrt(math.pow(L_front_temp, 2) + math.pow(L, 2))
        rR_front = math.sqrt(math.pow(R_front_temp, 2) + math.pow(L, 2))

        # 速度, m/s
        vL_rear = v * rL_rear/r
        vR_rear = v * rR_rear/r
        vL_front = v * rL_front/r
        vR_front = v * rR_front/r

        # 角度, rad
        anL_front = math.atan2(L, L_front_temp) * math.copysign(1, w)
        anR_front = math.atan2(L, R_front_temp) * math.copysign(1, w)

    else:
        # 速度
        vL_rear = v
        vR_rear = v
        vL_front = v
        vR_front = v

        # 角度
        anL_front = rad
        anR_front = rad 

    anL_front = limsteer(anL_front, max_rad)  
    anR_front = limsteer(anR_front, max_rad)

    pub_vel_left_rear_wheel.publish(vL_rear)
    pub_vel_right_rear_wheel.publish(vR_rear)
    pub_vel_left_front_wheel.publish(vL_front)
    pub_vel_right_front_wheel.publish(vR_front)
    pub_pos_left_steering_hinge.publish(anL_front)
    pub_pos_right_steering_hinge.publish(anR_front)

    # 表格形式输出
    print(f"{'Wheel/Steering':<25}{'Value':<10}")
    print("-" * 35)
    print(f"{'Left Front Wheel Velocity : ':<25}{vL_front:.2f} m/s")
    print(f"{'Right Front Wheel Velocity: ':<25}{vR_front:.2f} m/s")
    print(f"{'Left Rear Wheel Velocity  : ' :<25}{vL_rear:.2f} m/s")
    print(f"{'Right Rear Wheel Velocity : ':<25}{vR_rear:.2f} m/s")
    print(f"{'Left Front Steering Angle : ':<25}{anL_front:.2f} rad")
    print(f"{'Right Front Steering Angle: ':<25}{anR_front:.2f} rad")


# 将 geometry_msgs/Twist 类型的消息 转换为 阿克曼小车模型四个轮子的速度和前轮转角
def set_speed(data):
    global L, B_f, B_r, max_rad
    
    pub_vel_left_rear_wheel = rospy.Publisher('/car/left_rear_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_right_rear_wheel = rospy.Publisher('/car/right_rear_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_left_front_wheel = rospy.Publisher('/car/left_front_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_right_front_wheel = rospy.Publisher('/car/right_front_wheel_velocity_controller/command', Float64, queue_size=1)

    pub_pos_left_steering_hinge = rospy.Publisher('/car/left_steering_hinge_position_controller/command', Float64, queue_size=1)
    pub_pos_right_steering_hinge = rospy.Publisher('/car/right_steering_hinge_position_controller/command', Float64, queue_size=1)
    
    v = data.linear.x  
    w = data.angular.z

    if v != 0 and w != 0:
        # r 为理想的中间轮胎的转弯半径
        r = math.fabs(v/w) 

        # math.copysign(x, y)：返回一个与 x 数值大小相同、与 y 符号相同的浮点数
        rL_rear = r - (math.copysign(1, w) * (B_r/2.0)) 
        rR_rear = r + (math.copysign(1, w) * (B_r/2.0))

        L_front_temp = r - (math.copysign(1, w) * (B_f/2.0)) 
        R_front_temp = r + (math.copysign(1, w) * (B_f/2.0))
        rL_front = math.sqrt(math.pow(L_front_temp, 2) + math.pow(L, 2))
        rR_front = math.sqrt(math.pow(R_front_temp, 2) + math.pow(L, 2))

        # 速度, m/s
        vL_rear = v * rL_rear/r
        vR_rear = v * rR_rear/r
        vL_front = v * rL_front/r
        vR_front = v * rR_front/r

        # 角度, rad
        anL_front = math.atan2(L, L_front_temp) * math.copysign(1, w)
        anR_front = math.atan2(L, R_front_temp) * math.copysign(1, w)

    else:
        # 速度
        vL_rear = v
        vR_rear = v
        vL_front = v
        vR_front = v

        # 角度
        anL_front = w
        anR_front = w        

    anL_front = limsteer(anL_front, max_rad)  
    anR_front = limsteer(anR_front, max_rad)
   
    pub_vel_left_rear_wheel.publish(vL_rear)
    pub_vel_right_rear_wheel.publish(vR_rear)
    pub_vel_left_front_wheel.publish(vL_front)
    pub_vel_right_front_wheel.publish(vR_front)
    pub_pos_left_steering_hinge.publish(anL_front)
    pub_pos_right_steering_hinge.publish(anR_front)

    # 表格形式输出
    print(f"{'Wheel/Steering':<25}{'Value':<10}")
    print("-" * 35)
    print(f"{'Left Front Wheel Velocity : ':<25}{vL_front:.2f} m/s")
    print(f"{'Right Front Wheel Velocity: ':<25}{vR_front:.2f} m/s")
    print(f"{'Left Rear Wheel Velocity  : ' :<25}{vL_rear:.2f} m/s")
    print(f"{'Right Rear Wheel Velocity : ':<25}{vR_rear:.2f} m/s")
    print(f"{'Left Front Steering Angle : ':<25}{anL_front:.2f} rad")
    print(f"{'Right Front Steering Angle: ':<25}{anR_front:.2f} rad")


def servo_commands():
    rospy.init_node('servo_commands', anonymous=True)

    # rospy.Subscriber("/ackermann_cmd", AckermannDriveStamped, set_throttle_steer) 
    rospy.Subscriber("/cmd_vel", Twist, set_speed)

    rospy.spin()

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

调试代码时，可以通过终端命令直接向话题中发布消息，便于调试：

# 以 10hz 的频率持续向话题发送消息
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 1.0]'

# 只向话题发布一帧消息，然后停止
rostopic pub /cmd_vel geometry_msgs/Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 1.0]' -1

Twist '[1.0, 0.0, 0.0]' '[0.0, 0.0, 1.0]' 测试结果：

Wheel/Steering           Value     
-----------------------------------
Left Front Wheel Velocity : 0.91 m/s
Right Front Wheel Velocity: 1.20 m/s
Left Rear Wheel Velocity  : 0.85 m/s
Right Rear Wheel Velocity : 1.15 m/s
Left Front Steering Angle : 0.38 rad
Right Front Steering Angle: 0.28 rad