ROS2-机器人建模
1. URDF
机器人建模可使用URDF（Unified Robot Description Format）统一机器人描述格式，URDF使用XML格式描述机器人文件。
URDF组成
<?xml version="1.0"?>
<robot name="fishbot">
<link></link>
<joint></joint>
......
</robot>
URDF由以下几个部分组成：
- 声明信息：第一行是xml的声明信息，第二部分是机器人的声明，通过robot标签就可以声明一个机器人模型
- link：link标签用来描述机器人的每一个连接的组件。
- joint：joint标签用来描述机器人连接组件之间的连接关系。
例如可以使用6个link和5个joint来描述一个小车机器人：
link标签
link标签用来描述机器人的每一个连接的组件，link标签包含以下几个子标签：
- visual：visual标签用来描述机器人的显示形状，visual标签包含以下几个子标签：
- geometry：geometry标签用来描述机器人的显示形状的几何信息，geometry标签包含以下几个子标签：- box：长方体，box标签包含以下属性：
- size：长方体的长度、宽度和高度。
- cylinder：圆柱体，cylinder标签包含以下几个属性：
- radius：半径。
- length：高度。
- sphere：球体，sphere标签包含以下几个属性：
- radius：球体的半径。
- mesh：第三方导出的网格模型文件，mesh标签包含以下几个属性：
- filename：文件路径。
- material：material标签用来描述机器人的显示形状的材质信息，material标签包含以下几个子标签：
- color：color标签用来描述机器人的显示形状的材质信息为一种颜色，color标签包含以下几个属性：
- rgba：红色、绿色、蓝色和透明度。
- origin：origin标签用来描述机器人的显示形状的位置和姿态信息，origin标签包含以下几个属性：
- xyz：x、y和z轴上的位置。
- rpy：姿态，rpy属性的值为roll、pitch和yaw三个欧拉角。
- collision：collision标签用来描述机器人的碰撞属性
- inertial：inertial标签用来描述机器人的惯性属性
示例：一个名为base_link的圆柱体的link标签如下：
<!-- base link -->
<link name="base_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.12" radius="0.10"/>
</geometry>
</visual>
</link>
joint标签
joint标签用来描述机器人连接组件之间的连接关系，主要描述：
- 父子之间的连接类型，包括是否固定的，可以旋转的等
- 父部件名字
- 子部件名字
- 父子之间相对位置
- 父子之间的旋转轴，绕哪个轴转
joint标签的type属性用来描述连接关系的类型，常见的连接类型有：
- fixed：固定连接，不允许运动。
- revolute：旋转关节，绕单轴旋转,角度有上下限,比如舵机0-180
- continuous：连续旋转关节，可以绕单轴无限旋转,比如自行车的前后轮
joint标签包含以下几个子标签：
- parent：连接关系中的父组件，使用link属性来描述父组件的名称。
- child：连接关系中的子组件，使用link属性来描述子组件的名称。
- origin：连接关系中的父子组件之间的相对位置和姿态信息，origin标签包含以下几个属性：
- xyz：x、y和z轴上的位置。
- rpy：姿态，rpy属性的值为roll、pitch和yaw三个欧拉角。 - axis：连接关系中的父子组件之间的旋转轴，axis标签包含属性xyz，表示围绕旋转的关节轴的矢量。
示例：一个连接小车主体base_link和雷达laser_link的joint标签如下：
<!-- laser joint -->
<joint name="laser_joint" type="fixed">
<parent link="base_link" />
<child link="laser_link" />
<origin xyz="0 0 0.075" />
</joint>
2. RVIZ2可视化
1. 建立功能包
ros2 pkg create fishbot_description --build-type ament_python
2. 创建urdf文件
cd fishbot_description
mkdir urdf
touch urdf/fishbot_base.urdf
fishbot_base.urdf文件内容如下：
<?xml version="1.0"?>
<robot name="fishbot">
<!-- base link -->
<link name="base_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.12" radius="0.10"/>
</geometry>
</visual>
</link>
<!-- laser link -->
<link name="laser_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.02"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.8" />
</material>
</visual>
</link>
<!-- laser joint -->
<joint name="laser_joint" type="fixed">
<parent link="base_link" />
<child link="laser_link" />
<origin xyz="0 0 0.075" />
</joint>
</robot>
3. 创建launch文件
mkdir launch
touch launch/display_rviz2.launch.py
display_rviz2.launch.py文件内容如下：
import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
package_name = 'fishbot_description'
urdf_name = "fishbot_base.urdf"
ld = LaunchDescription()
pkg_share = FindPackageShare(package=package_name).find(package_name)
urdf_model_path = os.path.join(pkg_share, f'urdf/{urdf_name}')
# 使用robot_state_publisher节点发布机器人模型信息robot_description，并将joint_states数据转换tf信息发布
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
arguments=[urdf_model_path]
)
# 使用joint_state_publisher_gui节点发布机器人关节状态信息，通过joint_states话题发布
joint_state_publisher_node = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui',
arguments=[urdf_model_path]
)
# 使用rviz2节点可视化机器人模型
rviz2_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
)
ld.add_action(robot_state_publisher_node)
ld.add_action(joint_state_publisher_node)
ld.add_action(rviz2_node)
return ld
4. 修改setup.py文件
from setuptools import find_packages, setup
from glob import glob # 导入头文件
import os # 导入头文件
package_name = 'fishbot_description'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), # 复制launch
(os.path.join('share', package_name, 'urdf'), glob('urdf/**')), # 复制URDF
],
package_data={'': ['py.typed']},
install_requires=['setuptools'],
zip_safe=True,
maintainer='root',
maintainer_email='root@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
extras_require={
'test': [
'pytest',
],
},
entry_points={
'console_scripts': [
],
},
)
5. 编译测试
安装ros版本对应的publisher节点
sudo apt install ros-kilted-joint-state-publisher-gui ros-kilted-robot-state-publisher
编译并启动
colcon build
source install/setup.bash
ros2 launch fishbot_description display_rviz2.launch.py
在rviz2界面中，点击左侧的Add按钮，添加RobotModel插件，在左侧的Displays界面中，将RobotModel->Description Topic改为/robot_description，并修改参考帧为base_link
看到小车机器人模型，红色圆柱base_link，黑色圆柱laser_link，使用固定关节连接在一起，laser_link位于base_link的上方：
rqt中显示节点关系图：
两轮差速模型
修改URDF，给小车添加两个轮子，与主躯干通过continuous关节连接，轮子可以绕轴无限旋转：
<?xml version="1.0"?>
<robot name="fishbot">
<!-- 躯干 -->
<link name="base_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.12" radius="0.10"/>
</geometry>
<material name="blue">
<color rgba="0.1 0.1 1.0 0.5" />
</material>
</visual>
</link>
<!-- 雷达 -->
<link name="laser_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.02"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>
</link>
<!-- 固定的雷达关节 -->
<joint name="laser_joint" type="fixed">
<parent link="base_link" />
<child link="laser_link" />
<origin xyz="0 0 0.075" />
</joint>
<!-- 惯性测量单元IMU -->
<link name="imu_link">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
</visual>
</link>
<!-- 固定的IMU关节 -->
<joint name="imu_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="0 0 0.02" />
</joint>
<!-- 左轮 -->
<link name="left_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.032"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>
</link>
<!-- 可绕y轴持续旋转的左轮关节 -->
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="left_wheel_link" />
<origin xyz="-0.02 0.10 -0.06" />
<axis xyz="0 1 0" />
</joint>
<!-- 右轮 -->
<link name="right_wheel_link">
<visual>
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
<geometry>
<cylinder length="0.04" radius="0.032"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>
</link>
<!-- 可绕y轴持续旋转的右轮关节 -->
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="right_wheel_link" />
<origin xyz="-0.02 -0.10 -0.06" />
<axis xyz="0 1 0" />
</joint>
<!-- 小车前方的支撑轮 -->
<link name="caster_link">
<visual>
<origin xyz="0 0 0" rpy="1.57079 0 0"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
<material name="black">
<color rgba="0.0 0.0 0.0 0.5" />
</material>
</visual>
</link>
<!-- 固定的支撑轮关节 -->
<joint name="caster_joint" type="fixed">
<parent link="base_link" />
<child link="caster_link" />
<origin xyz="0.06 0.0 -0.076" />
<axis xyz="0 1 0" />
</joint>
<!-- 虚拟关节，让车轮显示在地面之上 -->
<link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0.0 0.0 0.076" rpy="0 0 0"/>
</joint>
</robot>
重新编译运行，在rviz2界面中，点击左侧的Add按钮，添加RobotModel插件和TF插件。
可看到带有左右两个轮子和一个前方支撑轮的小车机器人模型
Joint State Publisher中新出现了两个关节left_wheel_joint和right_wheel_joint，拖动这两个关节可以控制左右轮子的旋转。
3. 控制车轮转动
在fishbot_describle包中新建节点发送关节位姿信息，控制车轮转动：
# 在fishbot_description/fishbot_description下新建节点文件
touch fishbot_description/fishbot_description/rotate_wheel.py
rotate_wheel.py文件内容如下：
#!/usr/bin/env python3
import threading
import time
# 导入ros库
import rclpy
from rclpy.node import Node
# 导入消息接口
from sensor_msgs.msg import JointState
# std_msgs/Header header : 时间戳信息 和 frame_id
# string[] name # 关节名称数组
# float64[] position # 关节位置数组
# float64[] velocity # 关节速度数组
# float64[] effort # 扭矩数组
class RotateWheelNode(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info(f"node {name} init..")
self.joint_states_publisher_ = self.create_publisher(JointState, "joint_states", 10) # 创建关节状态发布者
self._init_joint_states() # 初始化左右轮速率
self.pub_rate = self.create_rate(30) # 发布频率为30HZ
self.thread_ = threading.Thread(target=self._thread_pub)
self.thread_.start()
def _init_joint_states(self):
"""初始化左右轮速度"""
self.joint_speeds = [0.0,0.0]
self.joint_states = JointState()
self.joint_states.header.stamp = self.get_clock().now().to_msg()
self.joint_states.header.frame_id = ""
# 关节名称
self.joint_states.name = ['left_wheel_joint','right_wheel_joint']
# 关节转动的角度值，continuous类型的关节无上下限，初始值赋值0
self.joint_states.position = [0.0,0.0]
# 关节速度
self.joint_states.velocity = self.joint_speeds
# 力
self.joint_states.effort = []
def update_speed(self,speeds):
"""更新速度"""
self.joint_speeds = speeds
def _thread_pub(self):
"""线程工作函数，发布关节数据，原理是预测关节下一时刻的角度并发布消息使轮子转至该角度"""
# 某一段时间内轮子转动的角度 = （当前时刻-上一时刻）*两个时刻之间的轮子转速
last_update_time = time.time()
while rclpy.ok():
delta_time = time.time()-last_update_time # 经过的时间
last_update_time = time.time() # 更新时间
# 更新位置
self.joint_states.position[0] += delta_time*self.joint_states.velocity[0]
self.joint_states.position[1] += delta_time*self.joint_states.velocity[1]
# 更新速度
self.joint_states.velocity = self.joint_speeds
# 更新 header 时间戳
self.joint_states.header.stamp = self.get_clock().now().to_msg()
# 发布关节数据
self.joint_states_publisher_.publish(self.joint_states)
self.pub_rate.sleep() # rate.sleep()会动态改变自己的休眠时间以符合设定的频率
def main(args=None):
rclpy.init(args=args) # 初始化rclpy
node = RotateWheelNode("rotate_fishbot_wheel") # 新建一个节点
node.update_speed([15.0,-15.0]) # 设置速度
rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
rclpy.shutdown() # 关闭rclpy
注释掉launch文件中joint_state_publisher_gui节点的启动，避免和新建的rotate_wheel节点发布的关节状态信息冲突：
# ld.add_action(joint_state_publisher_node)
ld.add_action(robot_state_publisher_node)
ld.add_action(rviz2_node)
设置setup.py文件中entry_points，添加rotate_wheel节点：
entry_points={
'console_scripts': [
"rotate_wheel= fishbot_description.rotate_wheel:main"
],
},
编译并启动：
colcon build
# 运行关节数据发布节点
source install/setup.bash
ros2 run fishbot_description rotate_wheel
# 新建终端运行rviz2可视化
source install/setup.bash
ros2 launch fishbot_description display_rviz2.launch.py
在rviz2界面中，点击左侧的Add按钮，添加RobotModel插件和TF插件，设置后可看到小车轮子的转动
参数化实现
将轮子转速设置为参数
rotate_wheel.py文件修改如下：
#!/usr/bin/env python3
import threading
import time
# 导入ros库
import rclpy
from rclpy.node import Node
# 导入消息接口
from sensor_msgs.msg import JointState
# std_msgs/Header header : 时间戳信息 和 frame_id
# string[] name # 关节名称数组
# float64[] position # 关节位置数组
# float64[] velocity # 关节速度数组
# float64[] effort # 扭矩数组
class RotateWheelNode(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info(f"node {name} init..")
# 声明参数（可配置初始速度）
self.declare_parameter('left_wheel_speed', 0.0)
self.declare_parameter('right_wheel_speed', 0.0)
self.joint_states_publisher_ = self.create_publisher(JointState, "joint_states", 10) # 创建关节状态发布者
self._init_joint_states() # 初始化左右轮速率
self.pub_rate = self.create_rate(30) # 发布频率为30HZ
self.thread_ = threading.Thread(target=self._thread_pub)
self.thread_.start()
def _init_joint_states(self):
"""初始化左右轮速度"""
self.joint_speeds = [self.get_parameter('left_wheel_speed').value, self.get_parameter('right_wheel_speed').value]
self.joint_states = JointState()
self.joint_states.header.stamp = self.get_clock().now().to_msg()
self.joint_states.header.frame_id = ""
# 关节名称
self.joint_states.name = ['left_wheel_joint','right_wheel_joint']
# 关节转动的角度值，continuous类型的关节无上下限，初始值赋值0
self.joint_states.position = [0.0,0.0]
# 关节速度
self.joint_states.velocity = self.joint_speeds
# 力
self.joint_states.effort = []
def update_speed(self,speeds):
"""更新速度"""
self.joint_speeds = speeds
def _thread_pub(self):
"""线程工作函数，发布关节数据，原理是预测关节下一时刻的角度并发布消息使轮子转至该角度"""
# 某一段时间内轮子转动的角度 = （当前时刻-上一时刻）*两个时刻之间的轮子转速
last_update_time = time.time()
while rclpy.ok():
delta_time = time.time()-last_update_time # 经过的时间
last_update_time = time.time() # 更新时间
# 更新位置
self.joint_states.position[0] += delta_time*self.joint_states.velocity[0]
self.joint_states.position[1] += delta_time*self.joint_states.velocity[1]
# 更新速度
self.joint_states.velocity = [self.get_parameter('left_wheel_speed').value, self.get_parameter('right_wheel_speed').value]
# 更新 header 时间戳
self.joint_states.header.stamp = self.get_clock().now().to_msg()
# 发布关节数据
self.joint_states_publisher_.publish(self.joint_states)
self.pub_rate.sleep() # rate.sleep()会动态改变自己的休眠时间以符合设定的频率
def main(args=None):
rclpy.init(args=args) # 初始化rclpy
node = RotateWheelNode("rotate_fishbot_wheel") # 新建一个节点
rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
rclpy.shutdown() # 关闭rclpy
重新编译运行后，可使用rqt的Dynamic Reconfigure插件动态修改左右轮的速度参数，观察小车轮子的转动情况：
