ROS2使用Gazebo控制AGV_ros2 gazebo

目前国内关于ROS2的教程比较少，我记录一下再ROS2中使用Gazebo进行仿真的过程，也算是总结这一段时间关于ROS2的学习。

首先做的项目就是用控制小车运动，项目链接

1.差分小车模型的创建

使用差分小车是为了控制小车转弯，left wheel和right wheel是驱动轮，caster wheel负责保持稳定。在开始构建之前，为了可视化小车，需要了解
模型时如何展示的。

1. 小车模型由多个urdf或xacro文件构成，第一步就是用一个xacro文件把他们组合起来，然后用xacro命令将文件变成urdf文件。
2. 单个urdf文件通过robot_state_publisher这个Node,发布到/robot_description这个话题上，并把对应的关节的坐标变换传到tf树上
3. robot_state_publisher还有一个作用，就是接受/joint_states这个话题的数据作为输入，可以控制对应模型关节的运动。而joint_state_publisher_gui
   这个节点可以伪造这些关节的值，用于测试机器人是否能正常运动。

1.1 创建功能包

首先创建工作空间及功能包

mkdir -p car_ws/src

cd car_ws/src

ros2 pkg create --build-type ament_cmake car_description

colcon build

记得在.bashrc 中添加 source ~/car_ws/install/setup.bash

创建 launch urdf config meshes scripts等文件夹。然后配置CMakeLists.txt和package.xml

1.2 小车xacro编写模型

整个小车xacro文件分成Color、Base_Link、Chassis_Link、LEFT WHEEL_Link、RIGHT WHEEL_Link、CASTER WHEEL_Link，6个模块组成

5个link中每一个都是由Visual、Collision、 Inertia以及对应的joint组成，在编写的过程中可以实时使用rviz来显示.

在launch文件夹中编写一个launch文件，直接运行：ros2 launch car_description car.launch.py
即可显示（注意不要同时使用joint_state_publisher_gui和joint_state_publisher）

2.在Gazebo中使用小车

2.1 模型导入Gazebo

Gazebo的文件配置较为麻烦。先简单分析一下：

在Gazebo中进行仿真，需要use_sim_time，使用仿真时间统一所有节点的时间

使用 ros2 launch gazebo_ros gazebo.launch.py 开启一个新Gazebo世界

使用 ros2 run gazebo_ros spawn_entity.py -topic robot_description -entity car 。可以将robot_description这个话题中的模型加载到Gazebo世界，取名为car
1
2
3
4
5

因此可以编写一个launch文件，一键启动：ros2 launch car_description car_gazebo.launch.py

直接将robot_description加载到gazebo中，颜色信息无法体现，也无法控制小车运动，因此需要在原本的xacro文件中添加gazebo tag

关于颜色的tag

Gazebo/White

在每一个Link后面加上这个tag

2.2 控制小车运动

当我们想要使用ROS与Gazebo进行交互时，需要使用插件。例如ros2_control或者将要用的gazebo_ros_diff_drive（自带的），这样的插件可以通过C++来编写，具体可以参考 编写ros插件。

<gazebo>
    <plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
        <!-- Wheel Information -->
        <left_joint>left_wheel_joint</left_joint>
        <right_joint>right_wheel_joint</right_joint>
        <wheel_separation>0.35</wheel_separation>
        <wheel_diameter>0.1</wheel_diameter>
    
        <!-- Limits -->
        <max_wheel_torque>200</max_wheel_torque>
        <max_wheel_acceleration>10.0</max_wheel_acceleration>
    
        <!-- Output -->
        <odometry_frame>odom</odometry_frame>
        <robot_base_frame>base_link</robot_base_frame>

        <publish_odom>true</publish_odom>
        <publish_odom_tf>true</publish_odom_tf>
        <publish_wheel_tf>true</publish_wheel_tf>

    </plugin>
</gazebo>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

libgazebo_ros_diff_drive.so需要传入轮子名称、尺寸、限制，以及输出选择等

与前文小车控制流程不一样的是，现在小车的关节角由Gazebo插件发布，而不是通过GUI伪造。 gazebo_ros_diff_drive接受/cmd_vel这个话题的数据，发布需要的关节数据，并广播一个从名为"odom"的新坐标系（类似于世界坐标系的起始位置）到"base_link"的变换，以便让其他代码知道当前机器人的位置。

而运行ros2 run teleop_twist_keyboard teleop_twist_keyboard

可以直接用键盘来输入geometry_msgs/msg/Twist这种消息和小车对接。

为了避免不稳定的情况，给caster wheel添加一个很小的摩擦系数：

Gazebo/Black

2.2 控制小车到指定位置

小车的位置通过订阅/odom可以获得，控制小车的话题是/cmd_vel，通过下面的代码可以实现对小车的位置控制：

#! /usr/bin/env python3

import rclpy
import math
import numpy as np
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry


def euler_from_quaternion(quaternion):
    """
    Converts quaternion (w in last place) to euler roll, pitch, yaw
    quaternion = [x, y, z, w]
    """
    x = quaternion.x
    y = quaternion.y
    z = quaternion.z
    w = quaternion.w

    sinr_cosp = 2 * (w * x + y * z)
    cosr_cosp = 1 - 2 * (x * x + y * y)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    sinp = 2 * (w * y - z * x)
    pitch = np.arcsin(sinp)

    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

class CarController(Node):
    def __init__(self):
        super().__init__('car_controller')

        # 目标位置
        self.target_x = -2.0  # 目标 x 坐标
        self.target_y = 2.0  # 目标 y 坐标
        self.current_x = 0.0  # 当前 x 坐标
        self.current_y = 0.0  # 当前 y 坐标
        self.roll=0.0
        self.pitch=0.0
        self.yaw=0.0

        # 订阅当前位置
        self.odom_sub = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10
        )

        # 发布控制指令
        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        # 标记当前步骤
        self.current_step = 1

        # 角速度
        self.angular_velocity = math.pi / 2  # 90 度/秒
        self.timer=None
        self.first_yaw =None

    def odom_callback(self, odom_msg):
        self.current_x = odom_msg.pose.pose.position.x
        self.current_y = odom_msg.pose.pose.position.y
        orientation=odom_msg.pose.pose.orientation
        self.roll, self.pitch, self.yaw = euler_from_quaternion(orientation)
        self.get_logger().info('Current Position: x={:.2f}, y={:.2f}, roll={:.2f}, pitch={:.2f}, yaw={:.2f}'.format(self.current_x, self.current_y, self.roll, self.pitch, self.yaw))


        if self.current_step == 1:  # 走完 x 轴
            if abs(self.current_x - self.target_x) < 0.05:
                self.get_logger().info('X direction target position reached!')
                self.current_step = 2  # 进入第二步：旋转
                self.stop_car()
                return

            cmd_vel = Twist()
            if self.current_x < self.target_x:
                cmd_vel.linear.x = 0.5  # 正向线速度，可根据需要进行调整
            else:
                cmd_vel.linear.x = -0.5  # 反向线速度，可根据需要进行调整

            self.cmd_vel_pub.publish(cmd_vel)

        elif self.current_step == 2:  # 根据 y 轴坐标差选择旋转方向
            dy = self.target_y - self.current_y
            if dy > 0:
                angle = math.pi / 2  # 向左旋转 90 度
            else :
                angle = -math.pi / 2  # 向右旋转 90 度
            
            if not self.first_yaw:
                self.first_yaw=self.yaw+angle

            if abs(self.first_yaw-self.yaw)>0.05:
                cmd_vel = Twist()
                cmd_vel.linear.x = 0.0
                cmd_vel.angular.z = angle
                self.cmd_vel_pub.publish(cmd_vel)   
            else:
                cmd_vel = Twist()
                self.cmd_vel_pub.publish(cmd_vel)   
                self.current_step = 3      
            # if self.get_clock().now().nanoseconds/1e9-self.first_time<1.0:
            #     cmd_vel = Twist()
            #     cmd_vel.linear.x = 0.0
            #     cmd_vel.angular.z = angle
            #     self.cmd_vel_pub.publish(cmd_vel)
            # else:
            #     cmd_vel = Twist()
            #     self.cmd_vel_pub.publish(cmd_vel)
            #     self.current_step == 3

        elif self.current_step == 3:  # 走完 y 轴
            if abs(self.current_y - self.target_y) < 0.05:
                self.get_logger().info('Target position reached!')
                self.stop_car()
                return

            cmd_vel = Twist()
            cmd_vel.linear.x = 0.5  # 前进线速度，可根据需要进行调整
            self.cmd_vel_pub.publish(cmd_vel)

    def stop_car(self):
        cmd_vel = Twist()
        self.cmd_vel_pub.publish(cmd_vel)



    def move_to_target(self):
        while rclpy.ok():
            rclpy.spin_once(self)


def main(args=None):
    rclpy.init(args=args)
    car_controller = CarController()
    car_controller.move_to_target()


if __name__ == '__main__':
    main()
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
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146