目标：模拟一个用 URDF 建模的行走机器人，并在 Rviz 中查看。
教程级别：中级
时间：15 分钟
目录
背景
先决条件
任务
1 创建一个包
2 创建 URDF 文件
3 发布状态
4 创建启动文件
5 编辑 setup.py 文件
6 安装软件包
7 查看结果
摘要
背景
本教程将向您展示如何建模一个行走机器人，将状态发布为 tf2 消息，并在 Rviz 中查看模拟。首先，我们创建描述机器人组装的 URDF 模型。接下来，我们编写一个节点来模拟运动并发布 JointState 和变换。然后我们使用 robot_state_publisher 将整个机器人状态发布到 /tf2 。
先决条件
rviz2 https://index.ros.org/p/rviz2/
始终不要忘记在您打开的每个新终端中获取 ROS 2 的源。
source /opt/ros/jazzy/setup.bash
echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc任务
1. 创建一个包
创建目录：
mkdir -p second_ros2_ws/src然后创建包：
cd second_ros2_ws/src
ros2 pkg create --build-type ament_python --license Apache-2.0 urdf_tutorial_r2d2 --dependencies rclpy
cd urdf_tutorial_r2d2你现在应该看到一个 urdf_tutorial_r2d2 文件夹。接下来你将对其进行一些更改。
2 创建 URDF 文件
创建我们将存储一些资产的目录：
mkdir -p urdf下载 URDF file 并将其保存为 second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.urdf.xml 。下载 Rviz configuration file 并将其保存为 second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.rviz 。
https://docs.ros.org/en/jazzy/_downloads/872802005223ffdb75b1ab7b25ad445b/r2d2.urdf.xml
https://docs.ros.org/en/jazzy/_downloads/96d68aef72c4f27f32af5961ef48c475/r2d2.rviz
3 发布状态
现在我们需要一种方法来指定机器人所处的状态。为此，我们必须指定所有三个关节和整体里程计。
启动您喜欢的编辑器，并将以下代码粘贴到 second_ros2_ws/src/urdf_tutorial_r2d2/urdf_tutorial_r2d2/state_publisher.py 中
from math import sin, cos, pi # 从math模块导入sin, cos, pi函数
import rclpy # 导入rclpy库
from rclpy.node import Node # 从rclpy.node模块导入Node类
from rclpy.qos import QoSProfile # 从rclpy.qos模块导入QoSProfile类
from geometry_msgs.msg import Quaternion # 从geometry_msgs.msg模块导入Quaternion消息类型
from sensor_msgs.msg import JointState # 从sensor_msgs.msg模块导入JointState消息类型
from tf2_ros import TransformBroadcaster, TransformStamped # 从tf2_ros模块导入TransformBroadcaster和TransformStamped类
class StatePublisher(Node): # 定义StatePublisher类，继承自Node类
def __init__(self): # 初始化函数
rclpy.init() # 初始化rclpy
super().__init__('state_publisher') # 调用父类的初始化函数，并指定节点名称为'state_publisher'
qos_profile = QoSProfile(depth=10) # 创建一个QoSProfile对象，设置深度为10
self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile) # 创建一个发布者，发布JointState消息到'joint_states'话题
self.broadcaster = TransformBroadcaster(self, qos=qos_profile) # 创建一个TransformBroadcaster对象
self.nodeName = self.get_name() # 获取节点名称
self.get_logger().info("{0} started".format(self.nodeName)) # 打印日志信息，显示节点已启动
degree = pi / 180.0 # 将角度转换为弧度
loop_rate = self.create_rate(30) # 创建一个循环频率对象，设置频率为30Hz
# 机器人状态
tilt = 0. # 倾斜角度初始化为0
tinc = degree # 倾斜角度增量初始化为一个角度单位
swivel = 0. # 旋转角度初始化为0
angle = 0. # 角度初始化为0
height = 0. # 高度初始化为0
hinc = 0.005 # 高度增量初始化为0.005
# 消息声明
odom_trans = TransformStamped() # 创建一个TransformStamped对象
odom_trans.header.frame_id = 'odom' # 设置父坐标系为'odom'
odom_trans.child_frame_id = 'axis' # 设置子坐标系为'axis'
joint_state = JointState() # 创建一个JointState对象
try:
while rclpy.ok(): # 当rclpy处于正常状态时循环
rclpy.spin_once(self) # 处理一次ROS2的回调函数
# 更新joint_state
now = self.get_clock().now() # 获取当前时间
joint_state.header.stamp = now.to_msg() # 设置joint_state的时间戳
joint_state.name = ['swivel', 'tilt', 'periscope'] # 设置关节名称
joint_state.position = [swivel, tilt, height] # 设置关节位置
# 更新变换
# (以半径为2的圆运动)
odom_trans.header.stamp = now.to_msg() # 设置odom_trans的时间戳
odom_trans.transform.translation.x = cos(angle)*2 # 设置变换的x坐标
odom_trans.transform.translation.y = sin(angle)*2 # 设置变换的y坐标
odom_trans.transform.translation.z = 0.7 # 设置变换的z坐标
odom_trans.transform.rotation = \
euler_to_quaternion(0, 0, angle + pi/2) # 设置变换的旋转（欧拉角转换为四元数）
# 发送关节状态和变换
self.joint_pub.publish(joint_state) # 发布joint_state消息
self.broadcaster.sendTransform(odom_trans) # 发送odom_trans变换
# 创建新的机器人状态
tilt += tinc # 更新倾斜角度
if tilt < -0.5 or tilt > 0.0: # 如果倾斜角度超出范围，反向倾斜角度增量
tinc *= -1
height += hinc # 更新高度
if height > 0.2 or height < 0.0: # 如果高度超出范围，反向高度增量
hinc *= -1
swivel += degree # 更新旋转角度
angle += degree/4 # 更新角度
# 根据需要调整每次迭代的频率
loop_rate.sleep() # 休眠以维持循环频率
except KeyboardInterrupt: # 捕捉键盘中断异常
pass # 什么也不做
def euler_to_quaternion(roll, pitch, yaw): # 定义欧拉角转换为四元数的函数
qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2) # 计算四元数的x分量
qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2) # 计算四元数的y分量
qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2) # 计算四元数的z分量
qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2) # 计算四元数的w分量
return Quaternion(x=qx, y=qy, z=qz, w=qw) # 返回四元数对象
def main(): # 定义主函数
node = StatePublisher() # 创建StatePublisher节点
if __name__ == '__main__': # 如果脚本是直接运行的
main() # 调用主函数4 创建启动文件
创建一个新的 second_ros2_ws/src/urdf_tutorial_r2d2/launch 文件夹。打开你的编辑器并粘贴以下代码，将其保存为 second_ros2_ws/src/urdf_tutorial_r2d2/launch/demo_launch.py 。
import os # 导入os模块，用于文件和路径操作
from ament_index_python.packages import get_package_share_directory # 从ament_index_python.packages模块导入get_package_share_directory函数
from launch import LaunchDescription # 从launch模块导入LaunchDescription类
from launch.actions import DeclareLaunchArgument # 从launch.actions模块导入DeclareLaunchArgument类
from launch.substitutions import LaunchConfiguration # 从launch.substitutions模块导入LaunchConfiguration类
from launch_ros.actions import Node # 从launch_ros.actions模块导入Node类
def generate_launch_description(): # 定义generate_launch_description函数
use_sim_time = LaunchConfiguration('use_sim_time', default='false') # 创建LaunchConfiguration对象，默认值为'false'
urdf_file_name = 'r2d2.urdf.xml' # 定义URDF文件名
urdf = os.path.join(
get_package_share_directory('urdf_tutorial_r2d2'), # 获取urdf_tutorial_r2d2包的共享目录
urdf_file_name) # 拼接成URDF文件的完整路径
with open(urdf, 'r') as infp: # 打开URDF文件
robot_desc = infp.read() # 读取URDF文件内容
return LaunchDescription([ # 返回LaunchDescription对象
DeclareLaunchArgument(
'use_sim_time', # 声明一个启动参数'use_sim_time'
default_value='false', # 默认值为'false'
description='Use simulation (Gazebo) clock if true'), # 参数描述
Node(
package='robot_state_publisher', # 指定节点所在的包
executable='robot_state_publisher', # 指定可执行文件
name='robot_state_publisher', # 指定节点名称
output='screen', # 输出到屏幕
parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}], # 设置节点参数
arguments=[urdf]), # 传递URDF文件路径作为参数
Node(
package='urdf_tutorial_r2d2', # 指定节点所在的包
executable='state_publisher', # 指定可执行文件
name='state_publisher', # 指定节点名称
output='screen'), # 输出到屏幕
])5 编辑 setup.py 文件
您必须告诉 colcon 构建工具如何安装您的 Python 包。按如下方式编辑 second_ros2_ws/src/urdf_tutorial_r2d2/setup.py 文件：
包括这些导入语句
import os
from glob import glob
from setuptools import setup
from setuptools import find_packages将这两行添加到
data_files中
data_files=[
...
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
(os.path.join('share', package_name), glob('urdf/*')),
],修改
entry_points表，以便您稍后可以从控制台运行‘state_publisher’
'console_scripts': [
'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
],保存 setup.py 文件并进行更改。
import os # 导入os模块
from glob import glob # 从glob模块导入glob函数
from setuptools import setup # 从setuptools模块导入setup函数
from setuptools import find_packages # 从setuptools模块导入find_packages函数
package_name = 'urdf_tutorial_r2d2' # 定义包名称
setup(
name=package_name, # 设置包名称
version='0.0.0', # 设置包版本
packages=find_packages(exclude=['test']), # 查找包，排除'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(os.path.join('launch', '*launch.[pxy][yma]*'))), # 启动文件
(os.path.join('share', package_name), glob('urdf/*')), # URDF文件
],
install_requires=['setuptools'], # 设置依赖项
zip_safe=True, # 设置是否安全打包为zip文件
maintainer='cxy', # 设置维护者
maintainer_email='cxy@126.com', # 设置维护者邮箱
description='urdf tutorial r2d2', # 设置包描述
license='Apache-2.0', # 设置许可证
tests_require=['pytest'], # 设置测试依赖项
entry_points={
'console_scripts': [
'state_publisher = urdf_tutorial_r2d2.state_publisher:main' # 设置控制台脚本入口点
],
},
)6 安装软件包
cd second_ros2_ws
colcon build --symlink-install --packages-select urdf_tutorial_r2d2cxy@ubuntu2404-cxy:~/second_ros2_ws$ colcon build --symlink-install --packages-select urdf_tutorial_r2d2
Starting >>> urdf_tutorial_r2d2
--- stderr: urdf_tutorial_r2d2
/usr/lib/python3/dist-packages/setuptools/command/develop.py:40: EasyInstallDeprecationWarning: easy_install command is deprecated.
!!
********************************************************************************
Please avoid running ``setup.py`` and ``easy_install``.
Instead, use pypa/build, pypa/installer or other
standards-based tools.
See https://github.com/pypa/setuptools/issues/917 for details.
********************************************************************************
!!
easy_install.initialize_options(self)
/usr/lib/python3/dist-packages/setuptools/command/easy_install.py:363: UserWarning: Unbuilt egg for pytest-repeat [unknown version] (/usr/lib/python3/dist-packages)
self.local_index = Environment(self.shadow_path + sys.path)
---
Finished <<< urdf_tutorial_r2d2 [3.29s]
Summary: 1 package finished [3.50s]
1 package had stderr output: urdf_tutorial_r2d2
获取安装文件：
source install/setup.bash7 查看结果
启动包裹
ros2 launch urdf_tutorial_r2d2 demo_launch.py打开一个新的终端，然后运行 Rviz
rviz2 -d second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz请参阅用户指南以了解如何使用 Rviz。http://wiki.ros.org/rviz/UserGuide
摘要
您创建了一个 JointState 发布节点，并将其与 robot_state_publisher 结合以模拟行走机器人。这些示例中使用的代码最初来自这里https://github.com/benbongalon/ros2-migration/tree/master/urdf_tutorial 。
向本 ROS 1 教程https://wiki.ros.org/urdf/Tutorials/Using%20urdf%20with%20robot_state_publisher 的作者致谢，其中部分内容被重复使用。
![[Vulnhub] Pinkys-PalaceV1 Squid http proxy+SQI+BOF](https://img-blog.csdnimg.cn/img_convert/5d1b46bfeade8cc32a0a21d74473ff38.jpeg)
