重头开始看NAV2文档（一）——URDF

被遗忘的旋律.

已于 2024-10-29 17:19:48 修改

阅读量1.6k

点赞数 41

文章标签： ubuntu 机器人 1024程序员节

于 2024-10-23 10:40:43 首次发布

本文链接：https://blog.youkuaiyun.com/qq_56920196/article/details/143168564

版权

本文参考设置URDF和英文原版setting Up The URDF（属于是自己的屁话+cv文档原话）

本文具体代码可在sam_bot_description下的sam_bot_description功能包中找到。

URDF除了负责机器人的建模，也负责传感器到机器人底盘base_link的TF

1、编写代码

进入工作空间/src路径，创建功能包sam_bot_description

ros2 pkg create sam_bot_description

sam_bot_description/src下新建文件夹description，并在其中创建文件sam_bot_description.urdf，写并在其中入下面的代码：（不想看可以直接跳到后面看完整代码）

<?xml version="1.0"?>
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">


</robot>

定义一些常量，将其放在<robot> </robot>中间：

  <!-- Define robot constants -->
  <xacro:property name="base_width" value="0.31"/>  <!-- 底盘宽 -->
  <xacro:property name="base_length" value="0.42"/> <!-- 底盘长 -->
  <xacro:property name="base_height" value="0.18"/> <!-- 底盘高 -->

  <xacro:property name="wheel_radius" value="0.10"/><!-- 轮子半径 -->
  <xacro:property name="wheel_width" value="0.04"/> <!-- 轮子宽度 -->
  <xacro:property name="wheel_ygap" value="0.025"/> <!-- 轮子和底盘y轴的间距 -->
  <xacro:property name="wheel_zoff" value="0.05"/>  <!-- 定位后轮到z轴 -->
  <xacro:property name="wheel_xoff" value="0.12"/>  <!-- 定位后轮到x轴 -->

  <xacro:property name="caster_xoff" value="0.14"/> <!-- 定位前轮到x轴 -->

定义惯性属性

<!-- Define intertial property macros  -->
  <xacro:macro name="box_inertia" params="m w h d">    <!-- 定义矩形盒子的惯性 -->
    <inertial>                                         <!-- m惯性 w宽度 h高度 d深度 -->
      <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>        <!-- 惯性矩阵 -->
    </inertial>
  </xacro:macro>

  <xacro:macro name="cylinder_inertia" params="m r h">    <!-- 定义圆柱体的惯性 -->
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="sphere_inertia" params="m r">        <!-- 定义球体的惯性 -->
    <inertial>
      <mass value="${m}"/>
      <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
    </inertial>
  </xacro:macro>

定义底盘base_link。放在<robot> </robot>中间：

  <!-- Robot Base -->
  <link name="base_link">
    <visual>    <!-- 在visual中的参数只有视觉效果，没有物理属性 -->
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>    <!-- 颜色，a表示透明度 -->
      </material>
    </visual>

    <collision>    <!-- 定义碰撞 -->
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>
    
    <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/> <!-- 定义惯性 -->

  </link>

连接

  <!-- Robot Footprint -->
  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed"> <!-- fixed将base_joint固定在base_link上 -->
    <parent link="base_link"/>        <!-- 父 -->
    <child link="base_footprint"/>    <!-- 子。子固定在父上面-->
    <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>  <!-- 父与子之间的xyz位移和旋转，旋转用的是弧度制 -->
  </joint>

添加两个驱动轮：

  <!-- Wheels -->
  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">  <!-- prefix为关节名称添加前缀     x、y_prefix在x、y轴翻转轮子位置 -->
    <link name="${prefix}_link">
      <visual>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="Gray">
          <color rgba="0.5 0.5 0.5 1.0"/>
        </material>
      </visual>

      <collision>        <!-- 定义碰撞 -->
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>

      <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>    <!-- 定义惯性 -->

    </link>

    <joint name="${prefix}_joint" type="continuous">  <!-- continuous允许不限角度的旋转 -->
      <parent link="base_link"/>
      <child link="${prefix}_link"/>
      <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>   <!-- x、y_reflect控制轮子沿x、y轴进行翻转 -->
      <axis xyz="0 1 0"/>    <!-- 关节围绕y轴旋转 -->
    </joint>
  </xacro:macro>

  <xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />   <!-- 实例化 -->
  <xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />

在前部添加万向轮：

  <!-- Caster Wheel -->
  <link name="front_caster">
    <visual>
      <geometry>
        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
      </geometry>
    </collision>

    <xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>

  </link>

  <joint name="caster_joint" type="fixed">
    <parent link="base_link"/>
    <child link="front_caster"/>
    <origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
  </joint>

完整URDF代码：

<?xml version="1.0"?>
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">

  <!-- Define robot constants -->
  <xacro:property name="base_width" value="0.31"/>  <!-- 底盘宽 -->
  <xacro:property name="base_length" value="0.42"/> <!-- 底盘长 -->
  <xacro:property name="base_height" value="0.18"/> <!-- 底盘高 -->

  <xacro:property name="wheel_radius" value="0.10"/><!-- 轮子半径 -->
  <xacro:property name="wheel_width" value="0.04"/> <!-- 轮子宽度 -->
  <xacro:property name="wheel_ygap" value="0.025"/> <!-- 轮子和底盘y轴的间距 -->
  <xacro:property name="wheel_zoff" value="0.05"/>  <!-- 定位后轮到z轴 -->
  <xacro:property name="wheel_xoff" value="0.12"/>  <!-- 定位后轮到x轴 -->

  <xacro:property name="caster_xoff" value="0.14"/> <!-- 定位前轮到x轴 -->

  <!-- Define intertial property macros  -->
  <xacro:macro name="box_inertia" params="m w h d">    <!-- 定义矩形盒子的惯性 -->
    <inertial>                                         <!-- m惯性 w宽度 h高度 d深度 -->
      <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>        <!-- 惯性矩阵 -->
    </inertial>
  </xacro:macro>

  <xacro:macro name="cylinder_inertia" params="m r h">    <!-- 定义圆柱体的惯性 -->
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="sphere_inertia" params="m r">        <!-- 定义球体的惯性 -->
    <inertial>
      <mass value="${m}"/>
      <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
    </inertial>
  </xacro:macro>

  <!-- Robot Base -->
  <link name="base_link">
    <visual>    <!-- 在visual中的参数只有视觉效果，没有物理属性 -->
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>    <!-- 颜色 -->
      </material>
    </visual>

    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

    <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
  </link>

  <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>

  <!-- Robot Footprint -->
  <link name="base_footprint"/>
  <joint name="base_joint" type="fixed"> <!-- fixed将base_joint固定在base_link上 -->
    <parent link="base_link"/>        <!-- 父 -->
    <child link="base_footprint"/>    <!-- 子。子固定在父上面-->
    <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>  <!-- 父与子之间的xyz位移和旋转 -->
  </joint>

  <!-- Wheels -->
  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">  <!-- prefix为关节名称添加前缀     x、y_prefix在x、y轴翻转轮子位置 -->
    <link name="${prefix}_link">
      <visual>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="Gray">
          <color rgba="0.5 0.5 0.5 1.0"/>
        </material>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>

      <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
    </link>

    <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>

    <joint name="${prefix}_joint" type="continuous">  <!-- continuous允许不限角度的旋转 -->
      <parent link="base_link"/>
      <child link="${prefix}_link"/>
      <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>   <!-- x、y_reflect控制轮子沿x、y轴进行翻转 -->
      <axis xyz="0 1 0"/>    <!-- 关节围绕y轴旋转 -->
    </joint>
  </xacro:macro>

  <xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />   <!-- 实例化 -->
  <xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />

  <!-- Caster Wheel -->
  <link name="front_caster">
    <visual>
      <geometry>
        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
      </geometry>
    </collision>

    <xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
  </link>

  <joint name="caster_joint" type="fixed">
    <parent link="base_link"/>
    <child link="front_caster"/>
    <origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
  </joint>

</robot>

2、构建

编写package.xml：

在package.xml中添加：（最好放到<buildtool_depend>后面）

  <exec_depend>joint_state_publisher_gui</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>rviz</exec_depend>
  <exec_depend>xacro</exec_depend>

编写launch启动文件：

在sam_bot_description下创建文件夹launch并在其中创建文件display.launch.py，在其中写入代码：

import launch
from launch.substitutions import Command, LaunchConfiguration
import launch_ros
import os

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='sam_bot_description').find('sam_bot_description')
    default_model_path = os.path.join(pkg_share, 'src/description/sam_bot_description.urdf')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')

    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
    )
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
    )
    joint_state_publisher_gui_node = launch_ros.actions.Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
    )
    rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )

    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
                                            description='Flag to enable joint_state_publisher_gui'),
        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
                                            description='Absolute path to robot urdf file'),
        launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
                                            description='Absolute path to rviz config file'),
        joint_state_publisher_node,
        joint_state_publisher_gui_node,
        robot_state_publisher_node,
        rviz_node
    ])

修改CMakeList文件：

将下面的代码放在14行（大概）if(BUILD_TESTING)之前：

install(
  DIRECTORY src launch rviz
  DESTINATION share/${PROJECT_NAME}
)

配置rviz显示页面文件：

在sam_bot_description下创建文件夹rviz并在其中创建文件urdf_config.rviz，并在其中写入代码：

Panels:
  - Class: rviz_common/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
        - /RobotModel1/Links1
        - /TF1
      Splitter Ratio: 0.5
    Tree Height: 557
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Name: Grid
    - Alpha: 0.6
      Class: rviz_default_plugins/RobotModel
      Description Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /robot_description
      Enabled: true
      Name: RobotModel
      Visual Enabled: true
    - Class: rviz_default_plugins/TF
      Enabled: true
      Name: TF
      Marker Scale: 0.3
      Show Arrows: true
      Show Axes: true
      Show Names: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: base_link
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz_default_plugins/Interact
      Hide Inactive Objects: true
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera
    - Class: rviz_default_plugins/Measure
      Line color: 128; 128; 0
  Transformation:
    Current:
      Class: rviz_default_plugins/TF
  Value: true
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
      Name: Current View
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
    Saved: ~

ys启动！

还有一件事，安装一个小工具

 sudo apt install ros-humble-joint-state-publisher-gui

如果还报错缺什么东西安装就好

编译，source，然后启动：

ros2 launch sam_bot_description display.launch.py

左边RobotModel里面collision和Visual可以看机器人的视觉和碰撞体积

拖这个框里的滑条可以控制轮子转：