本节主要跟着小鱼老师的视频操作，不同的仿真平台有不同的建模语言，但是几乎都支持URDF。

本节使用URDF创建一个机器人模型。

6.2.1 帮机器人创建一个身体

URDF使用XML来描述机器人的结构和传感器、执行器等信息。

在chapt6/chap6_ws/src创建功能包:ros2 pkg create fishbot_description --build-type ament_cmake  --lience Apache-2.0

新功能包下创建urdf文件夹，新建文件：fisrt_robot.urdf. 代码如下：

<?xml version="1.0"?>
<robot name="first_robot">
    <!-- 机器人身体部分 -->
    <link name="base_link">
        <!-- 部件外观描述 -->
        <visual>
            <!-- 沿自己几何中心的偏移与旋转量 -->
            <origin xyz="0 0 0" rpy="0 0 0" />
            <!-- 几何形状 -->
            <geometry>
                <!-- 圆柱体，半径0.1m，高度 0.12m -->
                <cylinder length="0.12" radius="0.10" />
            </geometry>
            <!-- 材质子标签-蓝色 -->
            <material name="blue">
                <color rgba="0.1 0.1 1.0 0.5" />
            </material>
        </visual>
    </link>

    <!-- 机器人IMU部件 -->
    <link name="imu_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <box size="0.02 0.02 0.02" />
            </geometry>
        </visual>
        <material name="black">
            <color rgba="0 0 0 0.5" />
        </material>
    </link>

    <!-- 机器人关节 -->
    <joint name="imu_joint" type="fixed">
        <!-- 父部件 -->
        <parent link="base_link" />
        <!-- 子部件 -->
        <child link="imu_link" />
        <!-- 子部件相对父部件的平移和旋转 -->
        <origin xyz="0 0 0.03" rpy="0 0 0" />
    </joint>

</robot>

urdf_to_graphviz 转成pdf也不直观。

6.2.2  在RViz中显示机器人

之间终端输入rviz2,Display模块添加RobotModel,打开刚才的urdf文件,还好会提示TF错误。

安装依赖：

sudo apt install ros-$ROS_DISTRO-rotbot-state-publisher

sudo apt install ros-$ROS_DISTRO-joint-state-publisher

为了方便运行，使用launch启动节点。添加luanch文件夹，新建display_robot.launch.py.代码如下

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取默认路径
    urdf_tutorial_path = get_package_share_directory('fishbot_description')
    default_model_path = urdf_tutorial_path + '/urdf/first_robot.urdf'
    default_rviz_config_path = urdf_tutorial_path + '/config/display_model.rviz'
    # 为 Launch 声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', default_value=str(default_model_path),
        description='URDF 的绝对路径')
    # 获取文件内容生成新的参数
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['cat ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
    # 状态发布节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )
    # 关节状态发布节点
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
    )
    # RViz 节点
    rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', default_rviz_config_path]
    )
    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        joint_state_publisher_node,
        robot_state_publisher_node,
        rviz_node
    ])

视频上小鱼老师是先把default_rviz_config_path 注释掉。对于rviz参数也去掉。修改CMakeLists.txt,增加节点urdf、launch，构建，启动后：

再次在 Rviz的Display模块做设置，步骤参考视频或者书上。添加完效果如下：

把配置.rviz保存下来，再次修改launch启动文件,加上保存的文件，参见上面代码，就可达到上面的效果。

6.2.3 使用xacro 简化URDF

安装依赖：sudo apt install ros-$ROS_DISTRO-xacro

新建文件：fist_robot.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">   
 <!-- base -->
    <xacro:macro name="base_link" params="length radius">
        <link name="base_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${length}" radius="${radius}" />
                </geometry>              
                <material name="blue">
                    <color rgba="0.1 0.1 1.0 0.5" />
                </material>
            </visual>
        </link>
    </xacro:macro>
    <xacro:macro name="imu_link" params="imu_name xyz">
        <link name="${imu_name}_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.02 0.02 0.02" />
                </geometry>
            </visual>
            <material name="black">
                <color rgba="0 0 0 0.5" />
            </material>
        </link>
    
        <joint name="${imu_name}_joint" type="fixed">
            <!-- 父部件 -->
            <parent link="base_link" />
            <!-- 子部件 -->
            <child link="${imu_name}_link" />
            <!-- 子部件相对父部件的平移和旋转 -->
            <origin xyz="${xyz}" rpy="0 0 0" />
        </joint>
    </xacro:macro>    
    <xacro:base_link length="0.12" radius="0.1"/>
    <xacro:imu_link imu_name="imu_up" xyz="0 0 0.03"/>
    <xacro:imu_link imu_name="imu_down" xyz="0 0 -0.03"/>

</robot>

修改上一节6.2.2的代码把display_robot.launch.py里面。cat 替换为xacro.

构建，运行：bohu@bohu-TM1701:~/chapt6/chapt6_ws$ ros2 launch fishbot_description display_robot.launch.py model:=/home/bohu/chapt6/chapt6_ws/src/fishbot_description/urdf/first_robot.xacro

6.2.4 创建机器人部件及添加物理属性

这块代码比较长，就不一一贴出来，对比书上6.2.4，6.2.5,6.3章节。

小鱼老师讲解的过程是先添加主体，再加传感器、执行器：轮子、6.3又加了质量与惯性。

代码结构如下：

构建后，运行：ros2 launch fishbot_description display_robot.launch.py model:=/home/bohu/chapt6/chapt6_ws/install/fishbot_description/share/fishbot_description/urdf/fishbot/fish_robot.xacro

视觉效果如下：

只看质量：

只看惯性：