1.编写urdf文件
<robot name="mycar">
<!-- 添加base_footprint -->
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.001" />
</geometry>
</visual>
</link>
<!-- 2.添加底盘 -->
<!--
形状:圆柱
半径:0,1米
高度:0.08米
离地距:0.015米
-->
<!-- 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.035
长度:0.015
-->
<!-- 3.1.link -->
<link name="left_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" />
<!-- 关节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" />
<!-- 关节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" />
<!-- 关节z上的设置=车体高度/2 + 离地间距 -->
<origin xyz="0.08 0 -0.047" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<joint name="back2link" type="continuous">
<parent link="base_link" />
<child link="back_wheel" />
<!-- 关节z上的设置=车体高度/2 + 离地间距 -->
<origin xyz="-0.08 0 -0.0475" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
</robot>
2.编写launch文件:
<launch>
<!-- <1.在参数服务器中载入urdf> -->
<param name="robot_description" textfile="$(find urdf01_rviz)/urdf/urdf/demo05_test.urdf" />
<!-- 2.启动rviz -->
<node pkg="rviz" name="rviz" type="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" />
<!-- 3.添加关节状态发布节点 -->
<node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
<!-- 4.添加机器人状态发布节点 -->
<node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
<!-- 5.关节运动控制节点 -->
<node pkg="joint_state_publisher_gui" name="joint_state_publisher_gui" type="joint_state_publisher_gui" />
</launch>
3.进入~/demo01_ws/src/urdf01_rviz/urdf/urdf空间用下列语句查看是否有语法错误:
sudo apt install liburdfdom-tools
check_urdf demo05_test.urdf
4.通过下列语句生成该文件包含的节点文件
urdf_to_graphiz demo05_test.urdf
查看PDF文件
evince mycar.pdf