ROS综合学习记录（一）---cmd_vel转换为阿克曼模型的速度变换

0、问题描述：

1. 根据https://wenku.baidu.com/view/ae8058e880c758f5f61fb7360b4c2e3f57272506.html
   或者其他资料，推导Ackermann差速控制输出方法，用python或c++实现。
2. 改进控制器，可选：
   ① 使用L1或者Pure_pursuit针对性修改。
   ② 修改servo_commands.py （提示1：teb的输出为间歇脉冲式，要让小车稳定前进，控制量必须连续。提示2：servo_commands.py中AckermannMuxOutput会持续发送0信号，导致小车走走停停。）
   ③ 其他控制方法。

1、问题分析:

其实题目已经很清楚了，不需要过多解释，这里引用一下学长的解释：
第一个任务，修改ackermann模型输出的目的是让小车转弯更丝滑，直接在servo_commands.py里修改就可以了，有同学单独写了脚本（也没问题但是最后还是要合并到servo里哦）
第二个任务跟第一个不重合，第二个是优化cmd_vel的输出，默认10hz的输出频率，频率比较低gazebo仿真出来车会一耸一耸的，提高输出频率小车运行会更稳一些（可以考虑用rospy.Timer）；或者也可以另外加一个节点，用L1或者pure_pursuit控制小车循迹。（即 local_planner的local_plan或cmd_vel → cmd_vel优化输出/L1/Pure_pursuit → 阿克曼差速输出 → 关节pid控制器）

2、解决方案：

2.1Ackermann差速控制输出方法

2.1.1理解阿克曼模型

对于阿克曼模型的定义，网上有很多解释，这里给出两篇参考：
这是ROSwiki上的解释
这是比较清晰的解释
简单总结：对于没有万向轮的小车，在转弯时，要实现较为丝滑的转弯，需要控制四个轮子的转速和角度，尤其是前两个轮子

2.1.2适用阿克曼模型的teb_local_planner

了解了阿克曼模型，接下来就是如何解决这个问题，即将本地规划（localplanner）得到的cmd_vel转换为四个轮子的控制参数。
两个前轮转向角度和四个轮子速度计算：由于Twist消息只会给出线速度和角速度，而非线速度和转向角度，所以这种速度公式还需要转换一下,转向角度可以由轴距和转向半径的商的反正切得到。这样，核心的六个参数（四个轮子线速度和前两个轮子的角度）便得出来了。

这是另外一篇较为详细的公式计算和编程：同时在B站有对应的视频教程
注意事项：

• （1）、cmd_vel转递的是线速度和角速度，而我们需要变换成四个轮子的线速度和前轮的角度，注意，变换之后是角度而非角速度。直接通过三角函数变换即可，不过也可以通过修改参数实现：

• （2）、可以通过查找urdf或者xacro文件查看轴距和轮距，也可以通过在gazebo中查看四个轮子中前后和左右坐标差来计算。

2.2改正cmd_vel的输出频率，得到一个稳定的速度控制输出

2.2.1输出不稳定问题

在利用cmd_vel控制小车的输出时，通过rqt_plot可以看到小车的速度输出是类似一个个脉冲的输出，这就使得速度控制非常不稳定。

2.2.2原因分析

究其原因，是因为在控制小车速度的节点不止一个，还有一个键盘控制的节点也会发送速度控制的参数。在小车导航时，由于没有键盘按下，该节点就会持续发送速度为零的控制命令,这样，小车行驶就会很不稳定。

2.2.3解决思路(a)

提高发送cmd_vel的频率，从而使得cmd_vel发送更为频繁，这样，小车大多数时候接受的就是我们想要它接受的cmd_vel，而非键盘发送的消息。直接修改参数即可：

修改后效果如下：

2.2.4解决思路(b)

直接取消小车对键盘控制节点的订阅，这种方法简单粗暴，但效果喜人，用了的都说好，效果如下：

3、程序代码

#!/usr/bin/env python
# -*- 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

#flag_move = 0

#接受键盘消息控制小车的速度和角度
def set_throttle_steer(data):

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

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

    # Velocity is in terms of radians per second.
    # Want to go 1 m/s with a wheel of radius 0.05m. This translates to 19.97 radians per second, roughly 20.
    # However, at a multiplication factor of 20 speed is half of what it should be, so doubled to 40.
    throttle = data.drive.speed * 40.0
    steer = data.drive.steering_angle

    pub_vel_left_rear_wheel.publish(throttle)
    pub_vel_right_rear_wheel.publish(throttle)
    pub_vel_left_front_wheel.publish(throttle)
    pub_vel_right_front_wheel.publish(throttle)
    pub_pos_left_steering_hinge.publish(steer)
    pub_pos_right_steering_hinge.publish(steer)


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

#转换cmd_vel的Twist消息为阿克曼模型的四个轮子的速度和前轮角度
def set_speed(data):
    #global flag_move
    
    pub_vel_left_rear_wheel = rospy.Publisher('/racecar/left_rear_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_right_rear_wheel = rospy.Publisher('/racecar/right_rear_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_left_front_wheel = rospy.Publisher('/racecar/left_front_wheel_velocity_controller/command', Float64, queue_size=1)
    pub_vel_right_front_wheel = rospy.Publisher('/racecar/right_front_wheel_velocity_controller/command', Float64, queue_size=1)

    pub_pos_left_steering_hinge = rospy.Publisher('/racecar/left_steering_hinge_position_controller/command', Float64, queue_size=1)
    pub_pos_right_steering_hinge = rospy.Publisher('/racecar/right_steering_hinge_position_controller/command', Float64, queue_size=1)
    
    # Velocity is in terms of radians per second.
    # Want to go 1 m/s with a wheel of radius 0.05m. This translates to 19.97 radians per second, roughly 20.
    # However, at a multiplication factor of 20 speed is half of what it should be, so doubled to 40.
    x = data.linear.x  
    z = data.angular.z
    L = 0.335   #轴距
    T = 0.305   #两侧轮子之间的距离
    if z!=0 and x!=0:
        r=math.fabs(x/z) #转弯半径（车子中心到转弯的圆心）

        rL_rear = r-(math.copysign(1,z)*(T/2.0))   #r为小车中心的转弯半径，所以T需要除以2在叠加上去
        rR_rear = r+(math.copysign(1,z)*(T/2.0))
        rL_front = math.sqrt(math.pow(rL_rear,2)+math.pow(L,2))
        rR_front = math.sqrt(math.pow(rR_rear,2)+math.pow(L,2))
        vL_rear = x*rL_rear/r
        vR_rear = x*rR_rear/r
        vL_front = x*rL_front/r
        vR_front = x*rR_front/r
        anL_front = math.atan2(L,rL_front)*math.copysign(1,z)
        anR_front = math.atan2(L,rR_front)*math.copysign(1,z)
        
    else:
        vL_rear = x
        vR_rear = x
        vL_front =x
        vR_front =x
        anL_front = z
        anR_front = z
    anL_front = limsteer(anL_front,0.7)  #最大转弯角度的弧度为0.7
    anR_front = limsteer(anR_front,0.7)
   
    pub_vel_left_rear_wheel.publish(vL_rear*100)
    pub_vel_right_rear_wheel.publish(vR_rear*100)
    pub_vel_left_front_wheel.publish(vL_front*100)
    pub_vel_right_front_wheel.publish(vR_front*100)
    pub_pos_left_steering_hinge.publish(anL_front)
    pub_pos_right_steering_hinge.publish(anR_front)

def servo_commands():
    rospy.init_node('servo_commands', anonymous=True)
    #rospy.Subscriber("/racecar/ackermann_cmd_mux/output", AckermannDriveStamped, set_throttle_steer)      #取消了键盘控制节点发布消息的订阅
    rospy.Subscriber("/cmd_vel", Twist, set_speed)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

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