纯跟踪学习笔记（2）Gazebo环境测试学习

纯跟踪学习笔记（2）Gazebo环境测试学习

前言

本篇文章是学习某站上一up主讲解的纯跟踪算法实现，他是通过导入已有路径然后使用阿克曼模型的小车对路径进行跟踪，而我的目的是使用差速小车在导航时对局部路径进行跟踪，所以打算先学习他的整体实现思路然后依据我自己的要求进行改进。看评论区好多小伙伴直接运行他的代码行不通，会有一些问题，当然我也一样，我尽量把我自己碰见的问题以及解决方法都告诉大家。附上up主pp实现讲解视频。我用的是ubuntu20.04，大家可以参考一下。

正文

前期工作

首先先创建工作空间，然后将源码git clone到我们的工作目录下，再将我们需要的部分移动到catkin_ws/src下，最后catkin_make。

git clone https://github.com/czhherry/self-driving-vehicle-101.git
mkdir -p catkin_ws/src
mv self-driving-vehicle-101/13_path_following_part_3_pure_persuit/ catkin_ws/src
cd catkin_ws
catkin_make
1
2
3
4
5

到这一步应该都是没有问题的，若有问题比如缺少什么依赖根据终端自己安装一下。
然后我个人会比较喜欢把这个包的路径写到bashrc里面，也可以根据个人习惯在catkin_ws下

source devel/setup.bash
1

我就继续说我的过程了

sudo gedit ~/.bashrc
然后在最下方加上
source ~/catkin_ws/devel/setup.bash
1
2
3

然后保存退出再source一下。

主要流程&问题解决方案

主要是打开下面几个launch文件：
1、打开gazebo一个空的世界

roslaunch gazebo_ros empty_world.launch 
1

2、然后是打开小车的模型

roslaunch car_model spawn_car.launch 
1

在这里我的终端会报错

Traceback (most recent call last):
  File "/home/jason/catkin_ws/src/13_path_following_part_3_pure_persuit/waypoint_loader/scripts/waypoint_loader.py", line 7, in <module>
    from geometry_msgs.msg import Quaternion, PoseStamped
  File "/opt/ros/noetic/lib/python3/dist-packages/geometry_msgs/msg/__init__.py", line 1, in <module>
    from ._Accel import *
  File "/opt/ros/noetic/lib/python3/dist-packages/geometry_msgs/msg/_Accel.py", line 6, in <module>
    import genpy
  File "/opt/ros/noetic/lib/python3/dist-packages/genpy/__init__.py", line 34, in <module>
    from . message import Message, SerializationError, DeserializationError, MessageException, struct_I
  File "/opt/ros/noetic/lib/python3/dist-packages/genpy/message.py", line 48, in <module>
Traceback (most recent call last):
  File "/home/jason/catkin_ws/src/13_path_following_part_3_pure_persuit/car_model/scripts/cmdvel2gazebo.py", line 3, in <module>
    import yaml
ImportError    : import rospy
No module named yaml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

我一开始以为是没有yaml这个包，然后我就打算安装一下

sudo pip3 install pyyaml
1

但是发现我已经有这个包了，然后我去看他的script，我觉得应该是python版本的问题，于是我索引到spawn_car.launch最终会调用三个python脚本，将他们改成python3的格式，直接gpt，大家也可以下方自取。

cmdvel2gazebo.py

import rospy
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
import math

class CmdVel2Gazebo:
    def __init__(self):
        rospy.init_node('cmdvel2gazebo', anonymous=True)
        rospy.Subscriber('/smart/cmd_vel', Twist, self.callback, queue_size=1)
        self.pub_steerL = rospy.Publisher('/smart/front_left_steering_position_controller/command', Float64, queue_size=1)
        self.pub_steerR = rospy.Publisher('/smart/front_right_steering_position_controller/command', Float64, queue_size=1)
        self.pub_rearL = rospy.Publisher('/smart/rear_left_velocity_controller/command', Float64, queue_size=1)
        self.pub_rearR = rospy.Publisher('/smart/rear_right_velocity_controller/command', Float64, queue_size=1)
        # Initial velocity and tire angle are 0
        self.x = 0
        self.z = 0
        # Car Wheelbase (in m)
        self.L = 1.868
        # Car Tread
        self.T_front = 1.284
        self.T_rear = 1.284  # 1.386
        # How many seconds delay for the dead man's switch
        self.timeout = rospy.Duration.from_sec(0.2)
        self.lastMsg = rospy.Time.now()
        # Maximum steer angle of the "inside" tire
        self.maxsteerInside = 0.6
        # Turning radius for maximum steer angle just with the inside tire
        # tan(maxsteerInside) = wheelbase / radius --> solve for max radius at this angle
        rMax = self.L / math.tan(self.maxsteerInside)
        # Radius of inside tire is rMax, so radius of the ideal middle tire (rIdeal) is rMax + treadwidth / 2
        rIdeal = rMax + (self.T_front / 2.0)
        # Maximum steering angle for ideal middle tire
        # tan(angle) = wheelbase / radius
        self.maxsteer = math.atan2(self.L, rIdeal)
        # Loop
        rate = rospy.Rate(10)  # Run at 10Hz
        while not rospy.is_shutdown():
            self.publish()
            rate.sleep()

    def callback(self, data):
        # w = v / r
        self.x = data.linear.x / 0.3
        # Constrain the ideal steering angle such that the ackermann steering is maxed out
        self.z = max(-self.maxsteer, min(self.maxsteer, data.angular.z))
        self.lastMsg = rospy.Time.now()

    def publish(self):
        # Now that these values are published, we
        # Reset the velocity so that if we don't hear new
        # ones for the next time step that we time out; note
        # that the tire angle will not change
        # NOTE: We only set self.x to be 0 after 200ms of timeout
        delta_last_msg_time = rospy.Time.now() - self.lastMsg
        msgs_too_old = delta_last_msg_time > self.timeout
        if msgs_too_old:
            self.x = 0
            msgRear = Float64()
            msgRear.data = self.x
            self.pub_rearL.publish(msgRear)
            self.pub_rearR.publish(msgRear)
            msgSteer = Float64()
            msgSteer.data = 0
            self.pub_steerL.publish(msgSteer)
            self.pub_steerR.publish(msgSteer)
            return

        # The self.z is the delta angle in radians of the imaginary front wheel of ackerman model
        if self.z!= 0:
            T_rear = self.T_rear
            T_front = self.T_front
            L = self.L
            # self.v is the linear *velocity*
            r = L / math.fabs(math.tan(self.z))

            rL_rear = r - (math.copysign(1, self.z) * (T_rear / 2.0))
            rR_rear = r + (math.copysign(1, self.z) * (T_rear / 2.0))
            rL_front = r - (math.copysign(1, self.z) * (T_front / 2.0))
            rR_front = r + (math.copysign(1, self.z) * (T_front / 2.0))
            msgRearR = Float64()
            # The right tire will go a little faster when we turn left (positive angle)
            # The amount is proportional to the radius of the outside / ideal
            msgRearR.data = self.x * rR_rear / r
            msgRearL = Float64()
            # The left tire will go a little slower when we turn left (positive angle)
            # The amount is proportional to the radius of the inside / ideal
            msgRearL.data = self.x * rL_rear / r

            self.pub_rearL.publish(msgRearL)
            self.pub_rearR.publish(msgRearR)

            msgSteerL = Float64()
            msgSteerR = Float64()
            # The left tire's angle is solved directly from geometry
            msgSteerL.data = math.atan2(L, rL_front) * math.copysign(1, self.z)
            self.pub_steerL.publish(msgSteerL)

            # The right tire's angle is solved directly from geometry
            msgSteerR.data = math.atan2(L, rR_front) * math.copysign(1, self.z)
            self.pub_steerR.publish(msgSteerR)
        else:
            # If we are not turning
            msgRear = Float64()
            msgRear.data = self.x
            self.pub_rearL.publish(msgRear)
            # msgRear.data = 0;
            self.pub_rearR.publish(msgRear)

            msgSteer = Float64()
            msgSteer.data = self.z

            self.pub_steerL.publish(msgSteer)
            self.pub_steerR.publish(msgSteer)


if __name__ == '__main__':
    try:
        CmdVel2Gazebo()
    except rospy.ROSInterruptException:
        pass
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122

transform_publisher.py

#!/usr/bin/env python3  
import rospy
import tf
from geometry_msgs.msg import PoseStamped

class TransformPublisher:
    def __init__(self):
        rospy.init_node('transform_publisher')
        rospy.Subscriber('/smart/center_pose', PoseStamped, self.pose_cb, queue_size=1)
        rospy.spin()

    def pose_cb(self, msg):
        pose = msg.pose.position
        orientation = msg.pose.orientation
        br = tf.TransformBroadcaster()
        br.sendTransform((pose.x, pose.y, pose.z),
                         (orientation.x, orientation.y, orientation.z, orientation.w),
                         rospy.Time.now(),
                         'base_link', 'world')


if __name__ == "__main__":
    try:
        TransformPublisher()
    except:
        rospy.logwarn("Cannot start transform publisher")
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

vehicle_pose_and_velocity_updater.py

#!/usr/bin/env python3

import os
import numpy as np
from math import cos, sin

from geometry_msgs.msg import PoseStamped, TwistStamped

from gazebo_msgs.msg import ModelStates, LinkStates

import tf
import rospy

class vehicle_pose_and_velocity_updater:
    def __init__(self):
        rospy.init_node('vehicle_pose_and_velocity_updater', log_level=rospy.DEBUG)

        self.rear_pose_pub = rospy.Publisher('/smart/rear_pose', PoseStamped, queue_size=1)
        self.center_pose_pub = rospy.Publisher('/smart/center_pose', PoseStamped, queue_size=1)
        self.vel_pub = rospy.Publisher('/smart/velocity', TwistStamped, queue_size=1)

        rospy.Subscriber('/gazebo/model_states', ModelStates, self.model_cb, queue_size=1)

        rospy.spin()

    def model_cb(self, data):
        try:
            vehicle_model_index = data.name.index("smart")
        except ValueError:
            return
        vehicle_position = data.pose[vehicle_model_index]
        vehicle_velocity = data.twist[vehicle_model_index]
        orientation = vehicle_position.orientation
        # Convert the quaternion to an euler angle and get the yaw
        (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])
        time_stamp = rospy.Time.now()

        # Vehicle center position
        center_pose = PoseStamped()
        center_pose.header.frame_id = '/world'
        center_pose.header.stamp = time_stamp
        center_pose.pose.position = vehicle_position.position
        center_pose.pose.orientation = vehicle_position.orientation
        self.center_pose_pub.publish(center_pose)

        # Vehicle rear axle position
        rear_pose = PoseStamped()
        rear_pose.header.frame_id = '/world'
        rear_pose.header.stamp = time_stamp
        center_x = vehicle_position.position.x
        center_y = vehicle_position.position.y
        rear_x = center_x - cos(yaw) * 0.945
        rear_y = center_y - sin(yaw) * 0.945
        rear_pose.pose.position.x = rear_x
        rear_pose.pose.position.y = rear_y
        rear_pose.pose.orientation = vehicle_position.orientation
        self.rear_pose_pub.publish(rear_pose)

        # Vehicle velocity
        velocity = TwistStamped()
        velocity.header.frame_id = ''
        velocity.header.stamp = time_stamp
        velocity.twist.linear = vehicle_velocity.linear
        velocity.twist.angular = vehicle_velocity.angular
        self.vel_pub.publish(velocity)


if __name__ == "__main__":
	try:
		vehicle_pose_and_velocity_updater()
	except:
		rospy.logwarn("cannot start vehicle pose and velocity updater updater")
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74

然后就可以完美打开了，附个图
接下来，我们打开rviz看看效果

rviz
1

可惜的是我的rviz终端一直在发关于TF转换的警告TF_REPEATED_DATA ignoring data with redundant timestamp for frame base_link (parent world) at time 373.788000 according to authority unknown_publisher 且我在rviz中查看robot model也是有问题的。
在解决的过程中，首先发现自己的终端会有控制器那方面的错误，然后我检查是不是缺包，果然是这个问题。大家也可以执行以下命令检查一下

sudo apt-get install ros-noetic-gazebo-ros-control
sudo apt-get install ros-noetic-ros-control
sudo apt-get install ros-noetic-ros-controllers
1
2
3

后来发现打开车模型的launch文件以后，gazebo的终端还会报错：
[[ERROR] [1713598838.339755597, 6.374000000]: No p gain specified for pid. Namespace: /smart/gazebo_ros_control/pid_gains/rear_right_wheel_joint [ERROR] [1713598838.340352960, 6.374000000]: No p gain specified for pid. Namespace: /smart/gazebo_ros_control/pid_gains/rear_left_wheel_joint]
这个问题的解决需要打开smart_control_config.yaml，将最下面gazebo_ros_control那一块的注释都给取消掉，就没有问题了，附上修改完的代码

smart:
# controls the rear two tires based on individual motors
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
  rear_right_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: rear_right_wheel_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  rear_left_velocity_controller:
    type: velocity_controllers/JointVelocityController
    joint: rear_left_wheel_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  front_right_steering_position_controller:
    type: effort_controllers/JointPositionController
    joint: front_right_steering_joint
    pid: {p: 40000.0, i: 200.0, d: 1.0}    
  front_left_steering_position_controller:
    type: effort_controllers/JointPositionController
    joint: front_left_steering_joint
    pid: {p: 40000.0, i: 200.0, d: 1.0}
  gazebo_ros_control:
    pid_gains:
      rear_right_wheel_joint:
        p: 100.0
        i: 0.5
        d: 0.0
      rear_left_wheel_joint:
        p: 100.0
        i: 0.5
        d: 0.0
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

碰见了好多问题，下面的解决方案我先自己研究一下再告诉u大家，也欢迎大佬说下rviz无法显示的问题

未完待续。。。