本文介绍了在ROS2中使用Gazebo进行仿真的步骤，包括差分小车模型的创建、在Gazebo中导入和控制模型。通过创建xacro文件组合小车模型，使用robot_state_publisher和joint_state_publisher来管理关节状态，并在Gazebo中通过插件控制小车运动。最后展示了如何编写Python脚本控制小车到达指定位置。
目前国内关于ROS2的教程比较少，我记录一下再ROS2中使用Gazebo进行仿真的过程，也算是总结这一段时间关于ROS2的学习。
首先做的项目就是用控制小车运动，项目链接
1.差分小车模型的创建
使用差分小车是为了控制小车转弯，left wheel和right wheel是驱动轮，caster wheel负责保持稳定。在开始构建之前，为了可视化小车，需要了解
模型时如何展示的。
- 小车模型由多个urdf或xacro文件构成，第一步就是用一个xacro文件把他们组合起来，然后用xacro命令将文件变成urdf文件。
- 单个urdf文件通过robot_state_publisher这个Node,发布到/robot_description这个话题上，并把对应的关节的坐标变换传到tf树上
- 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
因此可以编写一个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>
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()
扫一扫
2438
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
