視覺化平臺RVIZ2與URDF建模語言

1.視覺化簡介

座標相關、雷射雷達相關、攝像頭相關的rviz2外掛

2.rviz2基本使用

以sudo apt install ros-[ROS_DISTRO]-desktop格式安裝ROS2時，RViz已經預設被安裝了。

sudo apt install ros-[ROS_DISTRO]-rviz2

備註： 命令中的 [ROS_DISTRO] 指代ROS2版本。

方式1：rviz2；

方式2：ros2 run rviz2 rviz2。

rviz2 啟動之後，預設介面如下：

1. 上部為工具欄：包括視角控制、預估位姿設定、目標設定等，還可以新增自定義外掛；

2. 左側為外掛顯示區：包括新增、刪除、複製、重新命名外掛，顯示外掛，以及設定外掛屬性等功能；

3. 中間為3D試圖顯示區：以視覺化的方式顯示新增的外掛資訊；

4. 右側為觀測視角設定區：可以設定不同的觀測視角；

5. 下側為時間顯示區：包括系統時間和ROS時間。

左側外掛顯示區預設有兩個外掛：

• Global Options：該外掛用於設定全域性顯示相關的引數，一般情況下，需要自行設定的是 Fixed Frame 選項，該選項是其他所有資料在視覺化顯示時所參考的全域性座標系；

• Global Status：該外掛用於顯示在 Global Options 設定完畢 Fixed Frame 之後，所有的座標變換是否正常。

最上面是選單區，左側是外掛顯示區，中間是3D除錯區，右側是視角切換區域，最下方是時間區ROS Time是ROS2時間，Wall Time是系統時間，Elapsed是Rviz2執行的時間。

可以儲存rviz2的配置，也可以開啟配置

設定顯示面板

可以設定平面網格的個數

可以設定豎直方向網格的個數

可以設定網格邊長是多大

改變sRGB的4個通道

可以改變視角

設定偏移量，如果Z是-1，那麼網格會相對於座標系下沉1個單位

Fixed name一般是根座標系的名稱

background color就是背景色

frame rate是座標系的釋出頻率

全域性狀態，當fixed name設定對後，就無警告了

視角切換(一般預設)

可以翻轉Z軸

常用外掛：

上述每一種外掛又包含了諸多屬性，可以通過設定外掛屬性來控制外掛的最終顯示效果。

Image是攝像頭資料外掛

LaserScan是雷射雷達資料外掛

TF是座標變換外掛

RobotModel是機器人模型外掛

ros2 run rviz2 rviz2 -d xxx.rviz
#可以读取自己保存的rviz配置

3.rviz2整合URDF基本流程

3.1.  案例分析

請呼叫如下命令，安裝案例所需的兩個功能包(可以控制機器人關節運動)：

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

終端下進入工作空間的src目錄，呼叫如下命令建立C++功能包。

ros2 pkg create cpp06_urdf --build-type ament_cmake

功能包下新建 urdf、rviz、launch、meshes目錄以備用，其中 urdf 目錄下再新建子目錄 urdf 與 xacro，分別用於儲存 urdf 檔案和 xacro 檔案。

launch存放launch檔案

urdf檔案裡面存放urdf三維模型檔案

meshes存放stl模型

xacro可以簡化urdf檔案，並且增強其靈活性

rviz存放rviz2的配置

3.2.  框架搭建

 <robot> name="hello_world"
   <link> name="base_link"
     <visual>
       <geometry>
         <box size="0.5 0.2 0.1"/>
       </geometry>
     </visual>
   </link>
 </robot>

  標準的XML檔案

from launch import LaunchDescription
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command,LaunchConfiguration
from launch.actions import DeclareLaunchArgument

#示例：ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo01_helloworld.urdf
def generate_launch_description():

    cpp06_urdf_dir = get_package_share_directory("cpp06_urdf")
    default_model_path = os.path.join(cpp06_urdf_dir,"urdf/urdf","demo01_helloworld.urdf")
    default_rviz_path = os.path.join(cpp06_urdf_dir,"rviz","display.rviz")
    model = DeclareLaunchArgument(name="model", default_value=default_model_path)

    # 加载机器人模型

    # 1.启动 robot_state_publisher 节点并以参数方式加载 urdf 文件
    robot_description = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description}]
    )

    # 2.启动 joint_state_publisher 节点发布非固定关节状态
    joint_state_publisher = Node(
        package="joint_state_publisher",
        executable="joint_state_publisher"
    )

    # rviz2 节点
    rviz2 = Node(
        package="rviz2",
        executable="rviz2"

        # arguments=["-d", default_rviz_path]
    )
    return LaunchDescription([
        model,
        robot_state_publisher,
        joint_state_publisher,
        rviz2
    ])

<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>ros2launch</exec_depend>

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

colcon build --packages-select cpp06_urdf

source install/setup.bash
ros2 launch cpp06_urdf display.launch.py

   小提示：

  在本章的後續案例中，所有實現都遵循上述步驟，在後續案例中我們只需要關注 urdf 實現即可，launch 檔案和 配置檔案無需修改。

3.3.  urdf檔案

  按ctrl+\生成註釋

因為安裝過urdf外掛，所以有提示，需要建立robot根標籤

第一個屬性為機器人名字

第二個有個xml namespace，指向xacro

第三個有個xml namespace，指向xacro，然後還有一個機器人名字

(暫時用最簡單的，也就是第一個)

link標籤叫連桿，也需要起個名字，連桿一般指剛體部分

link有個子集標籤，叫visual

visual標籤下要寫機器人形狀

然後該標籤下又有一個子集標籤叫geometry(幾何形狀)

然後又有子集標籤叫box(矩形體狀)後面的size後面對應長寬高

 <robot name="boxrobot"> 

    <link name="base_link"> 

      <visual>

        <geometry>

          <box size="1.0 0.5 0.1"/>
        </geometry>
      </visual>
    </link>
  </robot>

3.4.  xacro工具(將磁碟檔案載入到ROS2中的工具)

搜尋是否安裝過xacro

ros2 pkg list | grep -i xacro

如果列印了xacro說明安裝了，如果沒列印，則要手動安裝

sudo apt-get update
sudo apt-get install ros-humble-xacro

使用xacro讀取檔案

檔案裡的內容被輸出到了終端，咱們一般整合到launch檔案中。咱們在終端裡是隻能檢視內容，但是用launch就可以把檔案弄到節點裡，也就是整合到ROS2裡。

3.5.  launch核心實現

核心實現就三步，載入機器人模型，節點發布非固定關節的狀態，啟動rviz2節點

建立rviz2節點很簡單，就宣告下包名，宣告下executable。

載入機器人模型比較複雜，載入機器人模型，也要建立一個節點，

然後有引數，引數裡有個鍵叫robot_description，然後這個鍵對應一個值

值是ParameterValue物件，這個物件裡執行了一個指令，叫xacro，然後後面又有一個Launch配置，其實就是urdf檔案的路徑。

這個值，其實就是URDF檔案裡的內容，但是內容太長了，所以我們把它封裝成一個物件。

命令列，不能直接當物件引數值，所以還要封裝

Comand是專門封裝終端指令執行的

記得xacro後面要有空格，這裡填路徑

此時已經定位到cpp06_urdf的share路徑下的cpp06_urdf路徑了，返回的也就是該路徑的字串

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类

# from launch.actions import ExecuteProcess

# from launch.substitutions import FindExecutable

# 参数声明与获取

# from launch.actions import DeclareLaunchArgument

# from launch.substitutions import LaunchConfiguration

# 文件包含相关

# from launch.actions import IncludeLaunchDescription

# from launch.launch_description_sources import PythonLaunchDescriptionSource

# 分组相关

# from launch_ros.actions import PushRosNamespace

# from launch.actions import GroupAction

# 事件相关

# from launch.event_handlers import OnProcessStart,OnProcessExit

# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo

# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

p_value = ParameterValue(Command(["xacro ",get_package_share_directory("cpp06_urdf") + "/urdf/urdf/demo01_boxrobot.urdf"]))
robot_state_pub = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    parameters=[{"robot_description":p_value}]
)

rviz2 = Node(
    package="rviz2",
    executable="rviz2"
    )

def generate_launch_description():
    return LaunchDescription([robot_state_pub,rviz2])

點選左下角Add新增RobotModel外掛

新建一個座標系外掛，長沿著X，寬沿著Y，高沿著Z

3.6.  launch最佳化說明與實現

我們還需要最佳化三個點，第一個是開啟關節節點，第二是設定rviz2預設配置檔案，第三是Launch檔案中我們將讀取的urdf檔案寫死了，所以要最佳化結構。

還要啟動這個節點，來控制關節運動，可以改成joint_state_publisher_gui，出現圖形化介面。

儲存一下rviz2的配置

正常的指令是

ros2 run rviz2 rviz2 -d rviz2配置的路径

建立一個引數叫model，值是後面那一長串。

LaunchConfiguration是解析引數

記得要把model放在最前面，放在後面是不可以的，現在已經把路徑封裝完畢了。

現在啟動是正常啟動預設的urdf路徑。

解析非預設值的urdf，在終端裡也有類似於get_package_share_directory，以下就是(這裡引數model少寫了個L)要把引數值用反引號(ESC與TAB中間的按鍵)框起來。

ros2 pkg prefix --share cpp06_urdf

ros2 run launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/hahah.urdf

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类

# from launch.actions import ExecuteProcess

# from launch.substitutions import FindExecutable

# 参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

# 文件包含相关

# from launch.actions import IncludeLaunchDescription

# from launch.launch_description_sources import PythonLaunchDescriptionSource

# 分组相关

# from launch_ros.actions import PushRosNamespace

# from launch.actions import GroupAction

# 事件相关

# from launch.event_handlers import OnProcessStart,OnProcessExit

# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo

# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

model = DeclareLaunchArgument(name="model",default_value=get_package_share_directory("cpp06_urdf") + "/urdf/urdf/demo01_boxrobot.urdf")

p_value = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
robot_state_pub = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    parameters=[{"robot_description":p_value}]
)

joint_state_pub = Node(
    package="joint_state_publisher",
    executable="joint_state_publisher"
    )

rviz2 = Node(
    package="rviz2",
    executable="rviz2",
    arguments=["-d",get_package_share_directory("cpp06_urdf") + "/rviz/urdf.rviz"]
    )

def generate_launch_description():
    return LaunchDescription([model,robot_state_pub,joint_state_pub,rviz2])

繼續最佳化最終的程式碼為：

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类

# from launch.actions import ExecuteProcess

# from launch.substitutions import FindExecutable

# 参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

# 文件包含相关

# from launch.actions import IncludeLaunchDescription

# from launch.launch_description_sources import PythonLaunchDescriptionSource

# 分组相关

# from launch_ros.actions import PushRosNamespace

# from launch.actions import GroupAction

# 事件相关

# from launch.event_handlers import OnProcessStart,OnProcessExit

# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo

# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

import os

cpp06_urdf_dir = get_package_share_directory("cpp06_urdf")

default_model_path = os.path.join(cpp06_urdf_dir,"urdf/urdf","demo01_boxrobot.urdf")
default_rviz_path = os.path.join(cpp06_urdf_dir,"rviz","urdf.rviz")

model = DeclareLaunchArgument(name="model",default_value=default_model_path)

p_value = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
robot_state_pub = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    parameters=[{"robot_description":p_value}]
)

# 关节信息节点

# joint_state_pub = Node(

#     package="joint_state_publisher",

#     executable="joint_state_publisher"

# )

# 关节信息节点图形界面(建议)
joint_state_pub = Node(
    package="joint_state_publisher_gui",
    executable="joint_state_publisher_gui"
)

rviz2 = Node(
    package="rviz2",
    executable="rviz2",

#    arguments=["-d",get_package_share_directory("cpp06_urdf") + "/rviz/urdf.rviz"]
    arguments=["-d",default_rviz_path]

    )

def generate_launch_description():
    return LaunchDescription([model,robot_state_pub,joint_state_pub,rviz2])

4.URDF語法

4.1.簡介

4.2.robot根標籤

對機器人進行分割，分割成幾個子集，比如一個子集描述頭，一個子集描述身子，最後再合成合集，成機器人

雖然我的子檔案和主檔案在邏輯上是有包含關係的，但是其實，他們都是單獨的urdf檔案。主檔案中，robot標籤的name屬性必須寫，子檔案可以不寫，如果寫，那麼子檔案name的值與主檔案的必須相同！

4.3.link標籤

4.3.1.簡介

每一個link都是剛體，都是獨立部件

Link是通過joint進行拼接的

Link主要包含三部分，Visual，Collision和Inertial

沒加尖括號的是屬性，加了的是標籤

（可選）：用於描述link的視覺化屬性，可以設定link的形狀（立方體、球體、圓柱等）。

• name （可選）：指定link名稱，此名稱會對映為同名座標系，還可以通過引用該值定位定位link。

• （必填）：用於設定link的形狀，比如：立方體、球體或圓柱。

  • ：立方體標籤，通過size屬性設定立方體的邊長，原點為其幾何中心。

  • ：圓柱標籤，通過radius屬性設定圓柱半徑，通過length屬性設定圓柱高度，原點為其幾何中心。

  • ：球體標籤，通過radius屬性設定球體半徑，原點為其幾何中心。

  • ：通過屬性filename引用“皮膚”檔案，為link設定外觀，該檔案必須是本地檔案。使用 package:///為檔名新增字首。

  • （可選）：用於設定link的相對偏移量以及旋轉角度，如未指定則使用預設值（無偏移且無旋轉）。

    • xyz ：表示x、y、z三個維度上的偏移量（以米為單位），不同數值之間使用空格分隔，如未指定則使用預設值（三個維度無偏移）。

    • rpy ：表示翻滾、俯仰與偏航的角度（以弧度為單位），不同數值之間使用空格分隔，如未指定則使用預設值（三個維度無旋轉）。

  • （可選）：視覺元素的材質。也可以在根標籤robot中定義material標籤，然後，可以在link中按名稱進行引用。

    • name （可選）：為material指定名稱，可以通過該值進行引用。

    • （可選）：rgba 材質的顏色，由代表red/green/blue/alpha 的四個數字組成，每個數字的範圍為 [0,1]。

    • （可選）：材質的紋理，可以由屬性filename設定。

當有多個Visual的時候，需要給Visual設定name，所以是個可選項。

Collision與模擬有關係，我們可以給我們的機器人的剛體設定一個碰撞區間，只要障礙物進入了區間，那麼就發生了碰撞，一般碰撞區間要比實際大小要大。

（可選）：link的碰撞屬性。可以與link的視覺屬性一致，也可以不同，比如：我們會通常使用更簡單的碰撞模型來減少計算時間，或者設定的值大於link的視覺屬性，以儘量避免碰撞。另外，同一連結可以存在多個 標籤例項，多個幾何圖形組合表示link的碰撞屬性。

• name （可選）：為collision設定名稱。

• （必須）：請參考visual標籤的geometry使用規則。

• （可選）：請參考visual標籤的origin使用規則。

Inertial是設定慣性矩陣的，也是和模擬有關係的。比如說機器人剎車，會出現前傾的情況，比如說慣性矩陣的重心高一些，那麼急剎車就會出現翻車的情況了。

（可選）：用於設定link的質量、質心位置和中心慣性特性，如果未指定，則預設為質量為0、慣性為0。

• （可選）：該位姿（平移、旋轉）描述了連結的質心框架 C 相對於連結框架 L 的位置和方向。

  • xyz ：表示從 Lo（連結框架原點）到 Co（連結的質心）的位置向量為 x L̂x + y L̂y + z L̂z，其中 L̂x、L̂y、L̂z 是連結框架 L 的正交單位向量。

  • rpy ：將 C 的單位向量 Ĉx、Ĉy、Ĉz 相對於連結框架 L 的方向表示為以弧度為單位的尤拉旋轉序列 (r p y)。注意：Ĉx、Ĉy、Ĉz 不需要與連桿的慣性主軸對齊。

• （必填）：通過其value屬性設定link的質量。

• （必填）：對於固定在質心座標系 C 中的單位向量 Ĉx、Ĉy、Ĉz，該連桿的慣性矩 ixx、iyy、izz 以及關於 Co（連桿的質心）的慣性 ixy、ixz、iyz 的乘積。

注意： 和 在模擬環境下才需要使用到，如果只是在 rviz2 中整合 urdf，那麼不必須為 link 定義這兩個標籤。

4.3.2.使用

<robot name="link_demo">

  <material name="yellow">
    <color rgba="0.7 0.7 0 0.8" />
  </material>
  <link name="base_link">
    <visual>

        <geometry>

            <box size="0.5 0.3 0.1" />

        </geometry>

        <origin xyz="0 0 0" rpy="0 0 0" />

        <material name="yellow"/>
    </visual>
  </link>
</robot>

ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo02_link.urdf

矩形體，球形，圓柱體

平移量，X平移1，Y和Z平移0，rpy旋轉度設定為0(旋轉度分別是尤拉角里的翻滾角Roll(繞X)，俯仰角Pitch(繞Y)，航向角Yaw(繞Z))

發現長方體在X上偏移了1

讓航向角Yaw(繞Z運動)為0.5 rad

設定翻滾角Roll(繞X運動)為0.5rad

設定俯仰角Pitch(繞Y運動)為0.5rad

sRGB是R,G,B,Alpha(浮點模型，所以範圍是0-1.0)

如果一個顏色要被用好幾次，可以封裝成一個類似於全域性變數的東西，然後在其他link中呼叫時，直接用 （這裡是個閉環，注意）

<robot name="link_demo">

    <material name="yellow">
      <color rgba="0.7 0.7 0 0.8" />
    </material>
    <link name="base_link">
      <visual>

          <geometry>

              <box size="0.5 0.3 0.1" />

          </geometry>

          <origin xyz="0 0 0" rpy="0 0 0" />

          <material name="yellow"/>
      </visual>
    </link>
  </robot>

4.3.3.使用補充

mesh標籤是引用皮膚檔案，一般是stl檔案，可以用SolidWorks匯出，可以看文件後面的SW2URDF

如果不會使用solidworks，可以學，這東西2天半就能學會，只是一個工具，只會畫圖沒有啥水平，最重要的還是機械設計比較難。想學的可以看兄弟社團機械學會微信公眾號的影片進行學習。

https://mp.weixin.qq.com/mp/homepage?__biz=MzI4MjkyMDgyMA==&hid=7&sn=1efc3d3cee0142970227785f767cc7c8&scene=18

當然，沒時間學習可以直接用別人畫好的機器人，在Github上搜索turtlebot3

這裡有已經匯出的模型，bases裡是外觀的模型，sensors是感測器的模型，wheels是輪子的模型。

我們克隆下倉庫，注意要克隆branch是ros2的分支。

我們引用一個就行了，看看效果即可，真正的應用還是要用SolidWorks通過SW2URDF外掛進行匯出

把專案裡的meshes/bases裡的burger_base.stl複製到我們WS裡的meshes目錄

filename是寫剛才的皮膚檔案，package://就是協議名，後面跟包名，也就是cpp06_urdf。然後跟功能包下的檔案路徑，也就是meshes/burger_base.stl（其實也就是share下的路徑）

因為是個三維模型，所以在填scale大小縮放時，需要填3個比例，咱們都填1.0即可。

為什麼顯示的模型會這麼大呢，因為rviz2以米為單位，而stl是以mm為單位，注意。在機械上，預設不說單位就都是mm，不要亂改單位，一般都要以mm為單位，咱們是做機器人的，要專業一些。

4.4.joint標籤

4.4.1.簡介

urdf 中的 joint 標籤用於描述機器人關節的運動學和動力學屬性，還可以指定關節運動的安全極限，機器人的兩個部件(分別稱之為 parent link 與 child link)以”關節“的形式相連線，不同的關節有不同的運動形式: 旋轉、滑動、固定、旋轉速度、旋轉角度限制....,比如:安裝在底座上的輪子可以360度旋轉，而攝像頭則可能是完全固定在底座上。

• name （必填）：為關節命名，名稱需要唯一。

• type （必填）：設定關節型別，可用型別如下：

  • continuous：旋轉關節，可以繞單軸無限旋轉。

  • revolute：旋轉關節，類似於 continues，但是有旋轉角度限制。

  • prismatic：滑動關節，沿某一軸線移動的關節，有位置極限。

  • planer：平面關節，允許在平面正交方向上平移或旋轉。

  • floating：浮動關節，允許進行平移、旋轉運動。

  • fixed：固定關節，不允許運動的特殊關節。

以下是子級標籤

• （必填）：指定父級link。

  • link （必填）：父級link的名字，是這個link在機器人結構樹中的名字。

• （必填）：指定子級link。

  • link （必填）：子級link的名字，是這個link在機器人結構樹中的名字。

• （可選）：這是從父link到子link的轉換，關節位於子link的原點。

  • xyz ：各軸線上的偏移量。

  • rpy ：各軸線上的偏移弧度。

• <axis> （可選）：如不設定，預設值為（1，0，0）。

  • xyz ：用於設定圍繞哪個關節軸運動。

• （可選）：關節的參考位置，用於校準關節的絕對位置。

  • rising （可選）：當關節向正方向移動時，該參考位置將觸發上升沿。

  • falling （可選）：當關節向正方向移動時，該參考位置將觸發下降沿。

• （可選）：指定接頭物理特性的元素。這些值用於指定關節的建模屬性，對模擬較為有用。

  • damping （可選）：關節的物理阻尼值，預設為0。

  • friction （可選）：關節的物理靜摩擦值，預設為0。

• （關節型別是revolute或prismatic時為必須的）：

  • lower （可選）：指定關節下限的屬性（旋轉關節以弧度為單位，稜柱關節以米為單位）。如果關節是連續的，則省略。

  • upper （可選）：指定關節上限的屬性（旋轉關節以弧度為單位，稜柱關節以米為單位）。如果關節是連續的，則省略。

  • effort （必填）：指定關節可受力的最大值。

  • velocity （必填）：用於設定最大關節速度（旋轉關節以弧度每秒 [rad/s] 為單位，稜柱關節以米每秒 [m/s] 為單位）。

• （可選）：此標籤用於指定定義的關節模仿另一個現有關節。該關節的值可以計算為value = multiplier * other_joint_value + offset。

  • joint （必填）：指定要模擬的關節的名稱。

  • multiplier （可選）：指定上述公式中的乘法因子。

  • offset （可選）：指定要在上述公式中新增的偏移量，預設為 0（旋轉關節的單位是弧度，稜柱關節的單位是米）。

• <safety_controller> （可選）：安全控制器。

  • soft_lower_limit （可選）：指定安全控制器開始限制關節位置的下關節邊界，此限制需要大於joint下限。

  • soft_upper_limit （可選）：指定安全控制器開始限制關節位置的關節上邊界的屬性，此限制需要小於joint上限。

  • k_position （可選）：指定位置和速度限制之間的關係。

  • k_velocity （必填）：指定力和速度限制之間的關係。

關節名稱是必填的，且是唯一的。

咱們最常用的是有限位的revolute型別的關節，continuous可無限旋轉的關節，fixed固定關節，這個根據具體的關節型別來填。

revolute一般用於工業機器人機械臂的關節，continuous比如舵輪結構的“關節”，fixed就是一些固定的不能運動的結構關節。

子集標籤很多，咱們用的最常用的就幾個，記住常用的即可。

parent標籤，link屬性指定父級link的名字。

child標籤類似。

這個軸，預設是1，0，0，以X軸進行旋轉，但是咱們一般是需要設定的，可通過SolidWorks設定基準軸進行設定。

剩下的標籤，都與關節型別有關，比如limit，如果關節型別是revolute，而且不設定limit，那麼在joint_state_publisher_gui裡是無法調關節的角度的。

其他標籤用到的時候再進行介紹。

4.4.2.練習

先把倆關節，單獨實現，然後再通過joint關節進行連線。

黃色的話，紅和綠要多一些。

這是紅色。

底盤Link就建立完畢了，

攝像頭Link也建立完畢了，

關節名字設定為camera2base_link，也就是攝像頭連線底座的joint，然後型別是360度都可以轉的continuous。

填入子級Link與父級Link

這樣兩個Link就通過該Joint連線到一起了。

還需要設定這倆選項，咱們先不設定，先看預設是什麼狀態。

colcon build --packages-select cpp06_urdf
source install/setup.bash
ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo03_joint.urdf

可以開啟TF看看座標系，勾上Show Names，發現是重合的。所以顯示效果不滿足咱們的邏輯業務。（預設狀態下），所以需要設定偏移量。

如果咱們想把攝像頭移動到車頭，如圖所示，在X上有偏移量，Y沒有，但是Z有。然後roll，pitch，yaw上沒有。

Z的高度就是1/2的底盤高度+1/2的攝像頭高度

咱們要繞Yaw旋轉，所以也就是繞Z軸旋轉，也就是001，注意一定得是整形，不能是浮點型。

想要看是否能旋轉，要開啟joint_state_publisher_gui，可以從launch中開啟，也可以從終端中直接開啟。

ros2 run joint_state_publisher_gui joint_state_publisher_gui

拖拽捲軸可改變攝像頭的Yaw

Randomize是隨機數，Center是回中(復位)

 <robot name="joint_demo">

  <material name="yellow">
    <color rgba="0.7 0.7 0 0.8" />
  </material>
  <material name="red">
    <color rgba="0.8 0.1 0.1 0.8" />
  </material>
  <link name="base_link">
    <visual>

        <geometry>
            <box size="0.5 0.3 0.1" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow"/>
    </visual>
  </link>

  <link name="camera">
      <visual>
          <geometry>
              <box size="0.02 0.05 0.05" />
          </geometry>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <material name="red" />
      </visual>
  </link>

  <joint name="camera2baselink" type="continuous">
      <parent link="base_link"/>
      <child link="camera" />

      <origin xyz="0.2 0 0.075" rpy="0 0 0" />
      <axis xyz="0 0 1" />
  </joint>

</robot>

4.4.3.joint_state_publisher

bug:Yaw不穩定，會一直回中。

其實是因為launch裡啟動的和終端裡啟動的衝突了。

解決方案：只啟動其中一個。

解決方案：直接在Launch裡啟動GUI版本的，這樣即可解決。(但是不建議)

建議方案：用非GUI版本的，因為以後，我們控制關節是用程式控制，而不是GUI控制。

如果只是想展示模型，用GUI，

如果想用程式控制，用普通版。

4.4.4.base_footprint

bug:機器人底盤半沉入地下

  <link name="base_footprint">
    <visual>
      <geometry>
          <sphere radius="0.001"/>
      </geometry>
    </visual>
  </link>

    <joint name="baselink2basefootprint" type="fixed">
      <parent link="base_footprint"/>
      <child link="base_link"/>
      <origin xyz="0.0 0.0 0.05"/>
    </joint>

Z的偏移量要填下沉底盤的距離，也就是整車底盤的一半。

修改參考座標系

其實這個最佳化，可以不做，影響不大。但是建議用有basefootprint版本的。

 <robot name="base_footprint_demo">

  <material name="yellow">
    <color rgba="0.7 0.7 0 0.8" />
  </material>
  <material name="red">
    <color rgba="0.8 0.1 0.1 0.8" />
  </material>

  <link name="base_footprint">
    <visual>
      <geometry>
          <sphere radius="0.001"/>
      </geometry>
    </visual>
  </link>

  <link name="base_link">
    <visual>

        <geometry>
            <box size="0.5 0.3 0.1" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow"/>
    </visual>
  </link>

  <joint name="baselink2basefootprint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.05"/>
  </joint>

  <link name="camera">
      <visual>
          <geometry>
              <box size="0.02 0.05 0.05" />
          </geometry>
          <origin xyz="0 0 0" rpy="0 0 0" />
          <material name="red" />
      </visual>
  </link>

  <joint name="camera2baselink" type="fixed">
      <parent link="base_link"/>
      <child link="camera" />

      <origin xyz="0.2 0 0.075" rpy="0 0 0" />
      <axis xyz="0 0 1" />
  </joint>

</robot>

4.5.練習

沒必要太深入練習，咱們可以直接用SolidWorks建模，更為友好。

 <robot name="exercise_demo">

  <material name="yellow">
    <color rgba="0.7 0.7 0 0.8" />
  </material>
  <material name="red">
    <color rgba="0.8 0.1 0.1 0.8" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 0.8" />
  </material>

  <link name="base_footprint">
    <visual>
      <geometry>
          <sphere radius="0.001"/>
      </geometry>
    </visual>
  </link>

  <link name="base_link">
    <visual>

        <geometry>
            <box size="0.2 0.12 0.07" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <material name="yellow"/>
    </visual>
  </link>

  <joint name="baselink2basefootprint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link"/>
    <origin xyz="0.0 0.0 0.05"/>
  </joint>

  <link name="front_left_wheel">
      <visual>
          <geometry>
              <cylinder radius="0.025" length="0.02"/>
          </geometry>
          <origin xyz="0 0 0" rpy="1.57 0 0" />
          <material name="gray" />
      </visual>
  </link>

  <joint name="frontleftwheel2baselink" type="continuous">
      <parent link="base_link"/>
      <child link="front_left_wheel" />

      <origin xyz="0.075 0.06 -0.025" rpy="0 0 0" />
      <axis xyz="0 1 0" />
  </joint>

  <link name="front_right_wheel">
    <visual>
        <geometry>
            <cylinder radius="0.025" length="0.02"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <material name="gray" />
    </visual>
  </link>

  <joint name="frontrightwheel2baselink" type="continuous">
      <parent link="base_link"/>
      <child link="front_right_wheel" />

      <origin xyz="0.075 -0.06 -0.025" rpy="0 0 0" />
      <axis xyz="0 1 0" />
  </joint>

  <link name="back_left_wheel">
    <visual>
        <geometry>
            <cylinder radius="0.025" length="0.02"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <material name="gray" />
    </visual>
  </link>

  <joint name="backleftwheel2baselink" type="continuous">
    <parent link="base_link"/>
    <child link="back_left_wheel" />

    <origin xyz="-0.075 0.06 -0.025" rpy="0 0 0" />
    <axis xyz="0 1 0" />
  </joint>

  <link name="back_right_wheel">
    <visual>
        <geometry>
            <cylinder radius="0.025" length="0.02"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.57 0 0" />
        <material name="gray" />
    </visual>
  </link>

  <joint name="backrightwheel2baselink" type="continuous">
    <parent link="base_link"/>
    <child link="back_right_wheel" />

    <origin xyz="-0.075 -0.06 -0.025" rpy="0 0 0" />
    <axis xyz="0 1 0" />
  </joint>

</robot>

終端執行

ros2 launch cpp06_urdf display.launch.py model:=`ros2 pkg prefix --share cpp06_urdf`/urdf/urdf/demo05_exercise.urdf

5.URDF工具

在 ROS2 中，提供了一些URDF檔案相關的工具，比如:

• check_urdf命令可以檢查複雜的 urdf 檔案是否存在語法問題；

• urdf_to_graphviz命令可以檢視 urdf 模型結構，顯示不同 link 的層級關係。

當然，要使用工具之前，請先安裝，安裝命令：sudo apt install liburdfdom-tools。

5.1.check_urdf 語法檢查

進入urdf檔案所屬目錄，呼叫：check_urdf urdf文件，如果不丟擲異常，說明檔案合法，否則非法。

示例，終端下進入功能包 cpp06_urdf 的 urdf/urdf 目錄，執行如下命令：

check_urdf demo05_exercise.urdf

urdf 檔案如無異常，將顯示urdf中link的層級關係，如下圖所示：

否則將會給出錯誤提示。

演示錯誤，

5.2.urdf_to_graphviz 結構檢視

進入urdf檔案所屬目錄，呼叫:urdf_to_graphviz urdf文件，當前目錄下會生成 pdf 檔案。

示例，終端下進入功能包 cpp06_urdf 的 urdf/urdf 目錄，執行如下命令：

urdf_to_graphviz demo05_exercise.urdf

當前目錄下，將生成以urdf中robot名稱命名的.pdf和.gv檔案，開啟pdf檔案會顯示如下圖內容：

在上圖中會以樹形結構顯示link與joint的關係。

注意： 該工具以前名為urdf_to_graphiz現建議使用urdf_to_graphviz替代。

urdf_to_graphiz是歷史版本，已經被廢棄，建議用urdf_to_graphviz。

黑色方框代表Link，藍色代表Joint，也會展示平移量和旋轉度等資訊。

6.SW2URDF

6.1.solidworks簡介

SolidWorks是一種計算機輔助設計（CAD）和計算機輔助製造（CAM）軟體，由Dassault Systèmes SolidWorks Corp.開發。它主要用於工程設計和製造，可用於建立3D三維模型、進行裝配設計、進行工程分析和繪圖等。SolidWorks具有直觀的使用者介面和強大的功能，使工程師和設計師能夠快速而精確地設計複雜的零部件和裝配體。該軟體廣泛應用於機械、航空航天、汽車、醫療裝置等行業，是工程設計領域的重要工具之一。

6.2.solidworks外掛sw2urdf介紹

sw2urdf外掛維基百科：

https://wiki.ros.org/sw_urdf_exporter

github下載連結：

https://github.com/ros/solidworks_urdf_exporter/releases

雖然github連結上寫著只支援到SW2021，但是目前發現最新版SW(2022、2024經測試)也是可以正常用的。

6.3.安裝sw2urdf

1. 去github上下載sw2urdf外掛（這個SW版本不用管，實測在後續的SW版本依然可用）

2. 檢視SW的安裝路徑

比如我的路徑"C:\Program Files\SOLIDWORKS Corp\SOLIDWORKS"

3. 開啟外掛安裝器，預設情況下會自動找到你的SW路徑進行安裝，如果沒有自動找到路徑安裝，那麼需要手動選取SW安裝路徑。

4. 檢視外掛是否安裝成功

如圖這樣就是安裝成功了。

6.4.匯出URDF與Meshes

1. 如圖是一個標準的SW裝配體圖(大家也可以嘗試自己手擼一個，裡面沒有傳動裝置也可以)

這個過程看不太懂的話，可以參考一下古月居老師的影片，結合本教程一起學習。 【SolidWorks模型匯出urdf（古月居老師）-嗶哩嗶哩】https://www.bilibili.com/video/BV1Tx411o7rH

2. 對joint關節建立基準軸

選擇關節的圓柱面或者其他面，對轉軸進行標定

如果建完基準軸，發現不能夠正常展示，那麼就開啟該選項。

建完所有系後

3. 開啟export as URDF選項

4. 需要先選擇base_link基底剛體

這裡的Global Origin可能需要自己選擇，就選擇Origin_Global即可，但是一般選擇automatically Generate即可。

5. 因為他的base_link通過一個joint連線了一個child_link，所以，這個選項要填1。

咱們把base_link連線的關節叫joint1，把joint1連線的更上面的剛體叫做link1,

然後參考基準軸選擇剛才咱們在這個關節處建立的基準軸1，然後joint type關節型別選擇revolute（有限位的關節，只有這樣，機器人關節才可以正常運動）

這裡的參考座標系系統可以選擇automatically Generate，也可以選擇Origin_Joint1。

然後再在link1上加一，建立link2。

後面的link3，link4新增步驟是一樣的，不再詳細展示。

6. 點選preview and export

7. 對關節進行限位設定

coordinates是座標系，axis是軸，如果這裡Axis座標顯示錯誤，那麼說明Coordinates或者Axis選擇錯了，請手動選擇正確的座標系或者基準軸。

如果轉軸生成錯誤，比如都是000，可以自己填一下。

比如這裡是X軸，可以把左輪全填-100,右輪100（這裡，僅供參考，chatgpt提供的解決方案）

8. 檢視矩陣，並點匯出urdf與meshes

這樣就成功生成ROS1的WS了，將WS移動至Linux系統。

6.5.將ROS1的WS轉化為ROS2的WS

1. 先將ROS1的WS移動到Linux系統上

2. 新建一個ROS2的WS

mkdir -p ros2_4axisrobot_ws/src
cd ros2_4axisrobot_ws/
colcon build
cd src
ros2 pkg create cpp01_urdf --build-type ament_cmake
cd ..
code .

3. 完善WS的目錄

在./src/cpp01_urdf路徑下建立launch、meshes、rviz、urdf等等資料夾，在urdf資料夾下再新建urdf和xacro資料夾

4. 複製ROS1的WS裡的URDF和Meshes到ROS2的WS中

5. 對CMake和package進行配置

在CMake中請新增

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

在Package.xml中請新增

  <exec_depend>rviz2</exec_depend>
  <exec_depend>xacro</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>joint_state_publisher</exec_depend>
  <exec_depend>ros2launch</exec_depend>

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(cpp01_urdf)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)

# uncomment the following section in order to fill in

# further dependencies manually.

# find_package(<dependency> REQUIRED)

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

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)

  # the following line skips the linter which checks for copyrights

  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)

  # the following line skips cpplint (only works in a git repo)

  # comment the line when this package is in a git repo and when

  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>cpp01_urdf</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="tungchiahui@gmail.com">tungchiahui</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <exec_depend>rviz2</exec_depend>
  <exec_depend>xacro</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>joint_state_publisher</exec_depend>
  <exec_depend>ros2launch</exec_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

6. 對launch檔案進行編寫

詳細過程請看URDF有關Launch的核心最佳化那一節

from launch import LaunchDescription
from launch_ros.actions import Node

# 封装终端指令相关类

# from launch.actions import ExecuteProcess

# from launch.substitutions import FindExecutable

# 参数声明与获取
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

# 文件包含相关

# from launch.actions import IncludeLaunchDescription

# from launch.launch_description_sources import PythonLaunchDescriptionSource

# 分组相关

# from launch_ros.actions import PushRosNamespace

# from launch.actions import GroupAction

# 事件相关

# from launch.event_handlers import OnProcessStart,OnProcessExit

# from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo

# 获取功能包下share目录或路径
from ament_index_python.packages import get_package_share_directory

from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import Command

import os

cpp01_urdf_dir = get_package_share_directory("cpp01_urdf")

default_model_path = os.path.join(cpp01_urdf_dir,"urdf/urdf","4axisrobot.urdf")
default_rviz_path = os.path.join(cpp01_urdf_dir,"rviz","urdf.rviz")

model = DeclareLaunchArgument(name="model",default_value=default_model_path)

p_value = ParameterValue(Command(["xacro ",LaunchConfiguration("model")]))
robot_state_pub = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    parameters=[{"robot_description":p_value}]
)

# 关节信息节点(建议)

# joint_state_pub = Node(

#     package="joint_state_publisher",

#     executable="joint_state_publisher"

# )

# 关节信息节点图形界面
joint_state_pub = Node(
    package="joint_state_publisher_gui",
    executable="joint_state_publisher_gui"
)

rviz2 = Node(
    package="rviz2",
    executable="rviz2",

#    arguments=["-d",get_package_share_directory("cpp06_urdf") + "/rviz/urdf.rviz"]
    arguments=["-d",default_rviz_path]

    )

def generate_launch_description():
    return LaunchDescription([model,robot_state_pub,joint_state_pub,rviz2])

7. 對URDF檔案進行修改

修改的內容主要有兩項，一個是meshes的路徑，一個是刪掉易引起報錯的註釋。

主要由於ROS1的WS和ROS2的WS路徑不同，所以，我們只需要修改有關路徑的內容，比如說Meshes的路徑

package://一般是share目錄，所以我們需要從cpp01_urdf這一級開始寫（share目錄需要編譯後才會顯示）

然後按Ctrl+H

然後還需要刪除掉urdf檔案剛開始的註釋，否則也會報錯

如下是替換完畢的URDF檔案：

<?xml version="1.0" encoding="utf-8"?>
<robot
  name="4axisrobot">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="-0.0819771145953119 -0.0394328123681598 -0.0605612328464761"
        rpy="0 0 0" />
      <mass
        value="7.43807257011133" />
      <inertia
        ixx="0.33173827700719"
        ixy="-1.91350798633049E-05"
        ixz="-2.38586363043925E-05"
        iyy="0.510937907009661"
        iyz="1.14706407748075E-05"
        izz="0.66107272632796" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link
    name="link1">
    <inertial>
      <origin
        xyz="0.19141 -0.050676 -0.030057"
        rpy="0 0 0" />
      <mass
        value="21.822" />
      <inertia
        ixx="0.50464"
        ixy="0.13295"
        ixz="0.087353"
        iyy="0.59136"
        iyz="-0.066053"
        izz="0.64081" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.79216 0.81961 0.93333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link1.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint1"
    type="revolute">
    <origin
      xyz="-0.081947 -0.039447 0.11191"
      rpy="0.40394 -1.5708 0" />
    <parent
      link="base_link" />
    <child
      link="link1" />
    <axis
      xyz="1 0 0" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="100"
      velocity="1" />
  </joint>
  <link
    name="link2">
    <inertial>
      <origin
        xyz="0.00711598521454873 0.430460941882332 -0.181074256126024"
        rpy="0 0 0" />
      <mass
        value="30.6115349647897" />
      <inertia
        ixx="2.84056101478768"
        ixy="0.00176700425270318"
        ixz="-0.000605507690039326"
        iyy="0.803420593303961"
        iyz="0.881921487314161"
        izz="2.25142021571439" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link2.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint2"
    type="revolute">
    <origin
      xyz="0.35141 -0.28119 0.13918"
      rpy="-2.0566 1.1982 1.5708" />
    <parent
      link="link1" />
    <child
      link="link2" />
    <axis
      xyz="-1 0 0" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="100"
      velocity="1" />
  </joint>
  <link
    name="link3">
    <inertial>
      <origin
        xyz="0.00112887115421612 0.0366346915274407 0.31720330934553"
        rpy="0 0 0" />
      <mass
        value="32.192726667578" />
      <inertia
        ixx="4.01515983126431"
        ixy="0.0013193752174692"
        ixz="0.0113529938102155"
        iyy="3.86196311673823"
        iyz="-0.336440022491883"
        izz="0.399977905540088" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link3.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link3.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint3"
    type="revolute">
    <origin
      xyz="0.3765 0.82481 -0.49591"
      rpy="2.5802 0 0" />
    <parent
      link="link2" />
    <child
      link="link3" />
    <axis
      xyz="1 0 0" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="100"
      velocity="1" />
  </joint>
  <link
    name="link4">
    <inertial>
      <origin
        xyz="0.162516620557178 -0.213844847423599 3.38133504529381E-07"
        rpy="0 0 0" />
      <mass
        value="12.1646007734167" />
      <inertia
        ixx="0.288208127188405"
        ixy="0.00080326659836716"
        ixz="8.72489392803044E-07"
        iyy="0.107000706575729"
        iyz="-1.48935044230573E-07"
        izz="0.295147863994059" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link4.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://cpp01_urdf/meshes/link4.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint4"
    type="revolute">
    <origin
      xyz="0 0.070837 0.99728"
      rpy="0.82315 -1.5708 0" />
    <parent
      link="link3" />
    <child
      link="link4" />
    <axis
      xyz="1 0 0" />
    <limit
      lower="-3.14"
      upper="3.14"
      effort="100"
      velocity="1" />
  </joint>
</robot>

8. 編譯

colcon build --packages-select cpp01_urdf

9. 更新終端環境

source install/setup.bash

10. 執行Launch

ros2 launch cpp01_urdf display.launch.py

11. 在Rviz2中新增外掛以及基本配置

12. 使用joint_state_publisher_gui可以調關節的角度

13. 儲存rviz2的配置到rviz資料夾中

首先點選rviz2的選單欄上的File，然後選擇Save Config as

可以去本節簡介看效果影片

6.6.注意事項（僅供參考，ChatGPT的解決方案）

6.6.1.座標系1

輪子要用continuous的關節，並且儘量自己選座標系，要求從後面看車的話，左是Y，前是X，上是Z。所有的座標系都要求。

可以先讓他幫你自動生成一個，你再去修改座標系，要簡單一些。

6.6.2.座標系讓車躺著

如果你的車在rviz2裡是側著睡覺的，那麼：

哈哈，沒錯，這種“整車側著躺屍”的情況 90%就是 STL 的座標軸方向搞錯了 。很多 CAD 工具匯出的 STL 模型預設是：

• Z 軸朝前（例如 SolidWorks：Z朝前，Y朝上，用右手座標系推X座標系位置，從車屁股後面看是朝左。）

• 或者 Y 軸朝上（例如 Blender）

而 ROS/URDF 的座標系統是：

• X 向前（前進方向）

• Y 向左（平移方向）

• Z 向上（重力方向）

6.6.3.車直走變旋轉，旋轉變直走

那說明你的轉軸有問題。

如果向前走成了左轉，那麼說明左輪的轉軸錯了，少了個符號，加上就行了。

比如這個010,你應該改成0-10

7.xacro

7.1.場景、作用與概念

場景

前面 URDF 檔案構建機器人模型的過程中，存在若干問題。

問題1：在設計關節的位置時，需要按照一定的規則計算，規則是固定的，但是在 URDF 中依賴於人工計算，存在不便，容易計算失誤，且當某些引數發生改變時，還需要重新計算。

問題2：URDF中的部分內容是高度重複的，比如車輪的設計實現，不同輪子只是部分引數不同，形狀、顏色、翻轉量都是一致的，在實際應用中，構建複雜的機器人模型時，更是易於出現高度重複的設計，按照一般的程式設計思想涉及到重複程式碼應該考慮封裝、複用，但是在之前的URDF檔案中並沒有相關操作。

......

如果在一般程式語言中遇到類似問題，我們可以通過變數結合函式解決。對應的，在 ROS 中也給出了類似程式設計的最佳化方案，該方案稱之為： Xacro（可以理解為urdf2.0） 。

概念

Xacro 是 XML Macros 的縮寫，Xacro 是一種 XML 巨集語言，是可程式設計的 XML。

Xacro 可以宣告變數，可以通過數學運算求解；可以使用流程控制控制執行順序；還可以通過宏封裝(可以想成函式)、複用功能，從而提高程式碼複用率以及程式的安全性。

作用

較之於純粹的 URDF 實現，可以編寫更安全、精簡、易讀性更強的機器人模型檔案，且可以提高編寫效率。

7.2.快速體驗

先安裝xacro

#humble版本
sudo apt install ros-humble-xacro
#jazzy版本
sudo apt install ros-jazzy-xacro

1.需求

使用xacro最佳化 6.4.4 URDF練習 中的小車底盤實現，需要使用變數封裝車輛引數，並使用 xacro 宏封裝輪子重複的程式碼並呼叫宏建立四個輪子(注意: 在此，演示 xacro 的基本使用，不必要生成合法的 URDF )。

2.實現

功能包cpp06_urdf的urdf/xacro目錄下，新建xacro檔案demo01_helloworld.urdf.xacro，並編輯檔案，輸入如下內容：

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:property name="wheel_radius" value="0.025" />
    <xacro:property name="wheel_length" value="0.02" />
    <xacro:property name="PI" value="3.1415927" />

    <xacro:macro name="wheel_func" params="wheel_name" >
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>
        </link>
    </xacro:macro>
    <xacro:wheel_func wheel_name="left_front"/>
    <xacro:wheel_func wheel_name="left_back"/>
    <xacro:wheel_func wheel_name="right_front"/>
    <xacro:wheel_func wheel_name="right_back"/>
</robot>

宏類似函式

params類似入口引數

子標籤類似函式體

終端下進入當前檔案所述目錄，輸入如下指令：

cd src/cpp06_urdf/urdf/xacro/
xacro demo01_helloworld.urdf.xacro
#或者
ros2 run xacro xacro demo01_helloworld.urdf.xacro

終端將會輸出如下內容（以下內容是純urdf）：

<?xml version="1.0" ?>

<robot name="mycar">
  <link name="left_front_wheel">
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <link name="left_back_wheel">
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <link name="right_front_wheel">
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
  <link name="right_back_wheel">
    <visual>
      <geometry>
        <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="1.57079635 0 0" xyz="0 0 0"/>
      <material name="wheel_color">
        <color rgba="0 0 0 0.3"/>
      </material>
    </visual>
  </link>
</robot>

顯然的，通過xacro我們方便的實現了程式碼複用。

7.2.1.語法

1.  简介

![](https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1611.webp)

  xacro 提供了可编程接口，类似于计算机语言，包括变量声明调用、函数声明与调用等语法实现。在使用 xacro 生成 urdf 时，根标签`robot`中**必须**包含命名空间声明:`xmlns:xacro="``http://wiki.ros.org/xacro``"`。

   **变量**

  变量用于封装 URDF 中的一些字段，比如: PAI 值，小车的尺寸，轮子半径 ....，变量的基本使用语法包括变量定义、变量调用、变量运算等。

  1.1变量定义

  语法格式：

```xml
<xacro:property name="变量名" value="变量值" />
```

  示例：

```xml
<xacro:property name="PI" value="3.1416"/>
<xacro:property name="wheel_radius" value="0.025"/>
<xacro:property name="wheel_length" value="0.02"/>
```

  1.2变量调用

  语法格式：

```xml
${变量名}
```

  示例：

```xml
<geometry>
    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
```

  1.3变量运算

  语法格式：

```xml
${数学表达式}
```

  示例：

```xml
<origin xyz="0 0 0" rpy="${PI / 2} 0 0" />
```

```xml
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="demo2_pro">

<xacro:property name="num1" value="10"/>
<xacro:property name="num2" value="20"/>

<car length="${num1}" width="${num2}"/>

<sum value="${num1 + num2}"/>
</robot>
```

![](https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1612.webp)

   **宏**

  类似于函数实现，提高代码复用率，优化代码结构，提高安全性。宏的基本使用语法包括宏的定义与调用。

  2.1宏定义

  语法格式：

```xml
<xacro:macro name="宏名称" params="参数列表(多参数之间使用空格分隔)">
    .....
    参数调用格式: ${参数名}
</xacro:macro>
```

  示例：

```xml
<xacro:macro name="wheel_func" params="wheel_name" >
    <link name="${wheel_name}_wheel">
        <visual>
            <geometry>
                <cylinder radius="${wheel_radius}" length="${wheel_length}" />
            </geometry>

            <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

            <material name="wheel_color">
                <color rgba="0 0 0 0.3" />
            </material>
        </visual>
    </link>
</xacro:macro>
```

  2.2宏调用

  语法格式：

```xml
<xacro:宏名称 参数1=xxx 参数2=xxx/>
```

  示例：

```xml
<xacro:wheel_func wheel_name="left_front"/>
<xacro:wheel_func wheel_name="left_back"/>
<xacro:wheel_func wheel_name="right_front"/>
<xacro:wheel_func wheel_name="right_back"/>
```

```xml
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="demo3_func">

    <xacro:macro name="get_sum" params="num1 num2">
        <sum value="${num1 + num2}"/>
    </xacro:macro>

    <xacro:get_sum num1="20" num2="30"/>
    <xacro:get_sum num1="70" num2="30"/>

</robot>
```

![](https://cdn.tungchiahui.cn/tungwebsite/assets/images/2023/12/30/image1613.webp)

   **文件**

機器人由多部件組成，不同部件可能封裝為單獨的 xacro 檔案，最後再將不同的檔案整合，組合為完整機器人，可以使用檔案包含實現。

語法格式：

<xacro:include filename="其他xacro文件" />

示例：

<robot name="car" xmlns:xacro="http://wiki.ros.org/xacro">
      <xacro:include filename="car_base.xacro" />
      <xacro:include filename="car_camera.xacro" />
      <xacro:include filename="car_laser.xacro" />
</robot>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="demo4_include">
    <xacro:include filename="demo02_base_pro.urdf.xacro"/>
    <xacro:include filename="demo03_base_func.urdf.xacro"/>
</robot>

但不建議這樣，建議父級xacro和子級xacro使用同樣的name。

7.3.練習

7.3.1.框架

1.需求

使用xacro建立一個四輪機器人模型，該模型底盤可以參考 6.4.4 URDF練習 中的實現，並且在底盤之上添加了相機與雷射雷達。相機與雷射雷達的尺寸引數、安裝位置可自定義。

2.實現分析

需求中的機器人模型是由底盤、攝像頭和雷達三部分組成的，那麼可以將每一部分都封裝進一個xacro檔案，最後再通過xacro檔案包含組織成一個完整的機器人模型。

3.實現

功能包cpp06_urdf的urdf/xacro目錄下，新建多個xacro檔案，分別為：

• car.urdf.xacro：用於包含不同機器人部件對應的xacro檔案；

• car_base.urdf.xacro：描述機器人底盤的xacro檔案；

• car_camera.urdf.xacro：描述攝像頭的xacro檔案；

• car_laser.urdf.xacro：描述雷達的xacro檔案。

編輯car.urdf.xacro檔案，輸入如下內容：

<robot name="car" xmlns:xacro="http://wiki.ros.org/xacro">
    <xacro:include filename="car_base.urdf.xacro"/>
    <xacro:include filename="car_camera.urdf.xacro"/>
    <xacro:include filename="car_laser.urdf.xacro"/>
</robot>

7.3.2.車體

編輯car_base.urdf.xacro檔案，輸入如下內容：

<robot xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:property name="PI" value="3.1416"/>

    <xacro:property name="base_link_x" value="0.2"/>
    <xacro:property name="base_link_y" value="0.12"/>
    <xacro:property name="base_link_z" value="0.07"/>

    <xacro:property name="distance" value="0.015"/>

    <xacro:property name="wheel_radius" value="0.025"/>
    <xacro:property name="wheel_length" value="0.02"/>

    <material name="yellow">
        <color rgba="0.7 0.7 0 0.8" />
    </material>
    <material name="red">
        <color rgba="0.8 0.1 0.1 0.8" />
    </material>
    <material name="gray">
        <color rgba="0.2 0.2 0.2 0.95" />
      </material>

    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="0.001"/>
            </geometry>
        </visual>
    </link>

    <link name="base_link">
        <visual>

            <geometry>
                <box size="${base_link_x} ${base_link_y} ${base_link_z}" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="yellow"/>
        </visual>
    </link>
    <joint name="baselink2basefootprint" type="fixed">
        <parent link="base_footprint"/>
        <child link="base_link"/>
        <origin xyz="0.0 0.0 ${distance + base_link_z / 2}"/>
    </joint>

    <xacro:macro name="wheel_func" params="wheel_name is_front is_left" >
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>
                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />
                <material name="gray"/>
            </visual>
        </link>
        <joint name="${wheel_name}2baselink" type="continuous">
            <parent link="base_link"  />
            <child link="${wheel_name}_wheel" />
            <origin xyz="${(base_link_x / 2 - wheel_radius) * is_front} ${base_link_y / 2 * is_left} ${(base_link_z / 2 + distance - wheel_radius) * -1}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>
    </xacro:macro>

    <xacro:wheel_func wheel_name="left_front" is_front="1" is_left="1" />
    <xacro:wheel_func wheel_name="left_back" is_front="-1" is_left="1" />
    <xacro:wheel_func wheel_name="right_front" is_front="1" is_left="-1" />
    <xacro:wheel_func wheel_name="right_back" is_front="-1" is_left="-1" />
</robot>

7.3.3.新增攝像頭

編輯car_camera.urdf.xacro檔案，輸入如下內容：

<robot xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:property name="camera_x" value="0.012" /> 
    <xacro:property name="camera_y" value="0.05" /> 
    <xacro:property name="camera_z" value="0.01" /> 
    <xacro:property name="camera_joint_x" value="${base_link_x / 2 - camera_x / 2}" /> 
    <xacro:property name="camera_joint_y" value="0.0" /> 
    <xacro:property name="camera_joint_z" value="${base_link_z / 2 + camera_z / 2}" /> 

    <link name="camera">
        <visual>
            <geometry>
                <box size="${camera_x} ${camera_y} ${camera_z}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="red" />
        </visual>
    </link>

    <joint name="camera2baselink" type="fixed">
        <parent link="base_link" />
        <child link="camera" />
        <origin xyz="${camera_joint_x} ${camera_joint_y} ${camera_joint_z}" />
    </joint>
</robot>

7.3.4.新增雷達

編輯car_laser.urdf.xacro檔案，輸入如下內容：

<robot xmlns:xacro="http://wiki.ros.org/xacro">

    <material name="blue">
        <color rgba="0.0 0.0 0.4 0.95" />
    </material>

    <xacro:property name="laser_length" value="0.03" /> 
    <xacro:property name="laser_radius" value="0.03" /> 
    <xacro:property name="laser_joint_x" value="0.0" /> 
    <xacro:property name="laser_joint_y" value="0.0" /> 
    <xacro:property name="laser_joint_z" value="${base_link_z / 2 + laser_length / 2}" /> 

    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <material name="blue" />
        </visual>
    </link>

    <joint name="laser2baselink" type="fixed">
        <parent link="base_link" />
        <child link="laser" />
        <origin xyz="${laser_joint_x} ${laser_joint_y} ${laser_joint_z}" />
    </joint>
</robot>

7.3.5.執行

編譯後，工作空間終端下呼叫如下命令執行：

# ROS Humble
ros2 launch cpp06_urdf display.launch.py model:=ros2 pkg prefix --share cpp06_urdf/urdf/xacro/car.urdf.xacro
#ROS Jazzy
colcon build
source install/setup.bash
ros2 run xacro xacro $(ros2 pkg prefix cpp06_urdf)/share/cpp06_urdf/urdf/xacro/car.urdf.xacro -o ./src/cpp06_urdf/urdf/urdf/car.urdf
ros2 launch cpp06_urdf display.launch.py model:=./src/cpp06_urdf/urdf/urdf/car.urdf

命令執行後，rviz2 中可以顯示與需求類似的機器人模型。

8.小結

目前只是空殼，雷射雷達和攝像頭以及輪子還都是空殼，到進階聯絡中，才可以實現作用。