一、概述
1.概念
2.作用
3.相关组件
二、URDF集成Rviz基本流程
1.创建功能包,导入依赖
2.编写 URDF 文件
·URDF文件
<robot name="mycar">
<link name="base_link">
<visual>
<geometry>
<box size="0.5 0.2 0.1"/>
</geometry>
</visual>
</link>
</robot>
3.在launch文件中集成 URDF 与 Rviz
·launch文件
<launch>
<!-- 1.在参数服务器载入 urdf 文件 -->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo01_helloworld.urdf" />
<!-- 2.启动 rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>
</launch>
4.在 Rviz 中显示机器人模型
5.优化 rviz 启动
·launch文件
<launch>
<!-- 1.在参数服务器载入 urdf 文件 -->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo02_link.urdf" />
<!-- 2.启动 rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>
</launch>
三、URDF语法详解
1.URDF语法详解01_robot
1)属性
2)子标签
2.URDF语法详解02_link
1)属性
2)子标签
3)案例
·URDF文件
<!-- 需求:设置不同形状的机器人部件 -->
<robot name="mycar">
<link name="base_link">
<!-- 可视化标签 -->
<visual>
<!-- 1.形状 -->
<geometry>
<!-- 1.1 立方体 -->
<!-- <box size="0.3 0.2 0.1" /> -->
<!-- 1.2 圆柱 -->
<!-- <cylinder radius="0.1" length="2" /> -->
<!-- 1.3 球体 -->
<!-- <sphere radius="1" /> -->
<!-- 1.4 皮肤 -->
<mesh filename="package://urdf01_rviz/meshes/autolabor_mini.stl" />
</geometry>
<!-- 2.偏移量与倾斜弧度 -->
<!--
xyz 设置机器人模型在 x y z 轴上的偏移量
rpy 用于设置倾斜弧度 x(翻滚) y(俯仰) z(偏航)
-->
<origin xyz="0 0 0" rpy="1.57 0 1.57" />
<!-- 3.颜色 -->
<!--
rgba:
r = red
g = green
b = blue
a = 透明度
四者取值 [0,1]
-->
<material name="car_color">
<color rgba="0 0 1 1" />
</material>
</visual>
</link>
</robot>
·launch文件
<launch>
<!-- 1.在参数服务器载入 urdf 文件 -->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo02_link.urdf" />
<!-- 2.启动 rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>
</launch>
3.URDF语法详解03_joint
1)属性
2)子标签
3)案例
·URDF文件
<!-- 需求:设置机器人底盘,并添加摄像头 -->
<robot name="mycar">
<!-- 1.底盘link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.3 0.2 0.1" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="car_color">
<color rgba="0.8 0.5 0 0.5" />
</material>
</visual>
</link>
<!-- 2.摄像头link -->
<link name="camera">
<visual>
<geometry>
<box size="0.02 0.05 0.05" />
</geometry>
<!-- 先使用默认(后期需要修改) -->
<origin xyz="0 0 0.025" rpy="0 0 0" />
<material name="camera_color">
<color rgba="0 0 1 0.5" />
</material>
</visual>
</link>
<!-- 3.关节 -->
<joint name="camera2bace" type="continuous">
<!-- 父级 link -->
<parent link="base_link" />
<!-- 子级 link -->
<child link="camera" />
<!-- 设置偏移量 -->
<origin xyz="0.12 0 0.05" rpy="0 0 0" />
<!-- 设置关节旋转参考的坐标轴 -->
<origin xyz="0 0 1" />
</joint>
</robot>
·launch文件
<launch>
<!-- 1.在参数服务器载入 urdf 文件 -->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo03_joint.urdf" />
<!-- 2.启动 rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>
<!--
只有上述两条语句:
表现:设置头显示位置与颜色异常
提示:No transform from [camera] to [base_link] 缺少 camera 到 base_link 的坐标变换
原因:rvis 中显示 URDF 时,必须发布不同部件之间的 坐标系 关系
解决:ROS中提供了关于机器人模型显示的坐标发布相关节点(两个)
-->
<!-- 关节状态发布节点 -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
<!-- 机器人状态发布节点 -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<!-- 添加控制关节运动的节点 -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>
4)base_footprint优化urdf
·URDF文件
<!-- 需求:设置机器人底盘,并添加摄像头 -->
<robot name="mycar">
<!-- 添加一个尺寸极小的 link,
再去关联初始 link 与 base_link,
关节的高度刚好和base_linkk下沉的高度一致(半个底盘高度) -->
<link name="base_footprint">
<visual>
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<!-- 1.底盘link -->
<link name="base_link">
<visual>
<geometry>
<box size="0.3 0.2 0.1" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="car_color">
<color rgba="0.8 0.5 0 0.5" />
</material>
</visual>
</link>
<!-- 2.摄像头link -->
<link name="camera">
<visual>
<geometry>
<box size="0.02 0.05 0.05" />
</geometry>
<!-- 先使用默认(后期需要修改) -->
<origin xyz="0 0 0.025" rpy="0 0 0" />
<material name="camera_color">
<color rgba="0 0 1 0.5" />
</material>
</visual>
</link>
<!-- 关联 base_footprint 与 base_link -->
<joint name="link2footprint" type="fixed">
<!-- 父级 link -->
<parent link="base_footprint" />
<!-- 子级 link -->
<child link="base_link" />
<!-- 设置偏移量 -->
<origin xyz="0 0 0.05" rpy="0 0 0" />
</joint>
<!-- 3.关节 -->
<joint name="camera2bace" type="continuous">
<!-- 父级 link -->
<parent link="base_link" />
<!-- 子级 link -->
<child link="camera" />
<!-- 设置偏移量 -->
<origin xyz="0.12 0 0.05" rpy="0 0 0" />
<!-- 设置关节旋转参考的坐标轴 -->
<origin xyz="0 0 1" />
</joint>
</robot>
·launch文件
<launch>
<!-- 1.在参数服务器载入 urdf 文件 -->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo04_base_footprint.urdf" />
<!-- 2.启动 rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>
<!--
只有上述两条语句:
表现:设置头显示位置与颜色异常
提示:No transform from [camera] to [base_link] 缺少 camera 到 base_link 的坐标变换
原因:rvis 中显示 URDF 时,必须发布不同部件之间的 坐标系 关系
解决:ROS中提供了关于机器人模型显示的坐标发布相关节点(两个)
-->
<!-- 关节状态发布节点 -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
<!-- 机器人状态发布节点 -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
</launch>
5)遇到问题以及解决
4.URDF练习
·URDF文件
<robot name="mycar">
<!-- 1.添加 base_footprint -->
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.001" />
</geometry>
</visual>
</link>
<!-- 2.添加底盘 -->
<!--
形状:圆柱
半径:0.1m
高度:0.08m
离地间距:0.015m
-->
<!-- 2-1.link -->
<link name="base_link">
<visual>
<geometry>
<cylinder radius="0.1" length="0.08" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="baselink_color">
<color rgba="1.0 0.5 0.2 0.5" />
</material>
</visual>
</link>
<!-- 2-2.joint -->
<joint name="link2footprint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<!-- 关节z上的位置 = 车体高度/2 + 离地高度 -->
<origin xyz="0 0 0.055" rpy="0 0 0" />
</joint>
<!-- 3.添加驱动轮 -->
<!--
形状:圆柱
半径:0.0325m
长度:0.015m
-->
<!-- 3-1.link -->
<link name="feft_wheel">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.015" />
</geometry>
<origin xyz="0 0 0" rpy="1.5708 0 0" />
<material name="wheel_color">
<color rgba="0 0 0 0.3" />
</material>
</visual>
</link>
<link name="right_wheel">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.015" />
</geometry>
<origin xyz="0 0 0" rpy="1.5708 0 0" />
<material name="wheel_color">
<color rgba="0 0 0 0.3" />
</material>
</visual>
</link>
<!-- 3-2.joint -->
<joint name="left2link" type="continuous">
<parent link="base_link" />
<child link="left_wheel" />
<!--
x 无偏移
y 车体半径
z z= 车体高度 / 2 + 离地高度 - 车轮半径
-->
<origin xyz="0 0.1 -0.0225" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<joint name="right2link" type="continuous">
<parent link="base_link" />
<child link="right_wheel" />
<!--
x 无偏移
y 车体半径
z z= 车体高度 / 2 + 离地高度 - 车轮半径
-->
<origin xyz="0 -0.1 -0.0225" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<!-- 4.添加万向轮 -->
<!--
形状:球
半径:0.0075m
-->
<!-- 4-1.link -->
<link name="front_wheel">
<visual>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="wheel_color">
<color rgba="0 0 0 0.3" />
</material>
</visual>
</link>
<link name="back_wheel">
<visual>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="wheel_color">
<color rgba="0 0 0 0.3" />
</material>
</visual>
</link>
<!-- 4-2.joint -->
<joint name="front2link" type="continuous">
<parent link="base_link" />
<child link="front_wheel" />
<origin xyz="0.08 0 -0.0475" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<joint name="back2link" type="continuous">
<parent link="base_link" />
<child link="back_wheel" />
<origin xyz="-0.08 0 -0.0475" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
</robot>
</robot>
·launch文件
<launch>
<!-- 1.在参数服务器载入 urdf 文件 -->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo05_test.urdf" />
<!-- 2.启动 rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz"/>
<!-- 3.添加关节状态发布节点 -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />
<!-- 4.添加机器人状态发布节点 -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<!-- 5.添加控制关节运动的节点 -->
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" />
</launch>