ROS2学习笔记6 URDF

I'm coming!!

于 2025-07-16 08:50:05 发布

阅读量623

点赞数 19

CC 4.0 BY-SA版权

文章标签：学习笔记

本文链接：https://blog.youkuaiyun.com/tech1001/article/details/148014981

由于赵虚左没有更新后续内容，从这章开始将转入小鱼的教程
视频
 docs
GIthub 仓库
URDF语法和ROS相同

6.2 新建 URDF 功能包

ros2 pkg create fishbot_description --build-type ament_cmake --license Apache-2.0

功能包不需要任何依赖
--license Apache-2.0表明使用的证书是Apache 2.0
urdf 如下

<?xml version="1.0"?>
<robot name="first_robot">
    <link name="base_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.10" length="0.12"/>
            </geometry>
            <material name="white">
                <color rgba="1.0 1.0 1.0 0.5"/>
            </material>
        </visual>
    </link>

    <link name="imu_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.02 0.02 0.02"/>
            </geometry>
            <material name="black">
                <color rgba="0.0 0.0 0.0 0.5"/>
            </material>
        </visual>
    </link>

    <joint name="imu_joint" type="fixed">
        <parent link="base_link"/>
        <child link="imu_link"/>
        <origin xyz="0.0 0.0 0.03" rpy="0.0 0.0 0.0"/>
    </joint>
</robot>

可以使用urdf_to_graphviz first_robot.urdf将 urdf 坐标关系用 PDF 查看

6.2.2 在 RVIZ 中显示机器人

安装包

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

公布包下新建 launch 目录，编写 python launch 文件

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 获取默认的 urdf 路径

    return launch.LaunchDescription([
        
    ])

配置 Cmake

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

全 Cmake

cmake_minimum_required(VERSION 3.8)
project(fishbot_description)

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)

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()

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

ament_package()

编译

完整 launch 文件

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os
import launch_ros.parameter_descriptions

def generate_launch_description():
    # 获取默认的 urdf 路径
    urdf_package_path = get_package_share_directory('fishbot_description')
    default_urdf_path = os.path.join(urdf_package_path,'urdf','first_robot.urdf')
    default_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot_model.rviz')
    # 声明一个 urdf 目录的参数，方便修改
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name="model",default_value=str(default_urdf_path),description='加载的模型文件路经'
    )
    # 通过文件路径，获取内容，并转换成参数值对象，以供传入 robot_state_publisher
    substitutions_commend_result = launch.substitutions.Command(['cat ',launch.
    substitutions.LaunchConfiguration('model')])
    robot_description_value = launch_ros.parameter_descriptions.ParameterValue(substitutions_commend_result,
    value_type=str)

    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}]
    )
    
    action_joint_state_publisher = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher'
    )

    action_rviz_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d',default_rviz_config_path]
    )

    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_joint_state_publisher,
        action_rviz_node
    ])

Cmake 修改

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

6.2.3 xacro 简化 urdf

新建 .xacro 文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="first_robot">
    <xacro:macro name="base_link" params="length radius">
        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 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.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.02 0.02 0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.5"/>
                </material>
            </visual>
        </link>

        <joint name="${imu_name}_joint" type="fixed">
            <parent link="base_link"/>
            <child link="${imu_name}_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.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.0 0.03"/>
    <xacro:imu_link imu_name="imu_down" xyz="0.0 0.0 -0.03"/>

</robot>

安装软件包
sudo apt install ros-$ROS_DISTRO-xacro

使用 xacro 可以将 xacro 转换为 urdf 输出(有错误会报错)
xacro /home/herrscher/chapt6/chapt6_ws/src/fishbot_description/urdf/first_robot.xacro

launch.py 改动

    # 通过文件路径，获取内容，并转换成参数值对象，以供传入 robot_state_publisher
    substitutions_commend_result = launch.substitutions.Command(['xacro ',launch.
    substitutions.LaunchConfiguration('model')])
    robot_description_value = launch_ros.parameter_descriptions.ParameterValue(substitutions_commend_result,
    value_type=str)

启动
ros2 launch fishbot_description display_robot.launch.py model:=/home/herrscher/chapt6/chapt6_ws/src/fishbot_description/urdf/first_robot.xacro

机器人构建

框架如下
在这里插入图片描述

caster

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="caster_xacro" params="caster_name xyz">
        <link name="${caster_name}_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <sphere radius="0.016"/>
                </geometry>
                <material name="yellow">
                    <color rgba="1.0 1.0 0.0 0.8"/>
                </material>
            </visual>
        </link>

        <joint name="${caster_name}_joint" type="fixed">
            <parent link="base_link"/>
            <child link="${caster_name}_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

whell

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="wheel_xacro" params="wheel_name xyz">
        <link name="${wheel_name}_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="1.57079 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.0325" length="0.04"/>
                </geometry>
                <material name="yellow">
                    <color rgba="1.0 1.0 0.0 0.8"/>
                </material>
            </visual>
        </link>

        <joint name="${wheel_name}_joint" type="continuous">
            <parent link="base_link"/>
            <child link="${wheel_name}_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
            <axis xyz="0.0 1.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

carema

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="carema_xacro" params="xyz">
        <link name="carema_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.02 0.10 0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.5"/>
                </material>
            </visual>
        </link>

        <joint name="carema_joint" type="fixed">
            <parent link="base_link"/>
            <child link="carema_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

imu

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="imu_xacro" params="xyz">
        <link name="imu_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.02 0.02 0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.5"/>
                </material>
            </visual>
        </link>

        <joint name="imu_joint" type="fixed">
            <parent link="base_link"/>
            <child link="imu_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

laser

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="laser_xacro" params="xyz">
        <!-- 雷达支撑杆 -->
        <link name="laser_cylinder_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.01" length="0.10"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 1.0"/>
                </material>
            </visual>
        </link>

        <link name="laser_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="0.02" length="0.02"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 1.0"/>
                </material>
            </visual>
        </link>

        <joint name="laser_joint" type="fixed">
            <parent link="laser_cylinder_link"/>
            <child link="laser_link"/>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        </joint>

        <joint name="laser_cylinder_joint" type="fixed">
            <parent link="base_link"/>
            <child link="laser_cylinder_link"/>
            <origin xyz="${xyz}" rpy="0.0 0.0 0.0"/>
        </joint>
    </xacro:macro>
</robot>

base

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="base_xacro" params="length radius">
        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
        </link>
    </xacro:macro>
</robot>

fishbot

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fishbot">
<!-- 基础部分 base_link -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro"/>
<!-- 传感器部分 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/carema.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/laser.urdf.xacro"/>
<!-- 执行器部分 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/wheel.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/caster.urdf.xacro"/>


<xacro:base_xacro length="0.12" radius="0.10" />
<xacro:imu_xacro xyz="0.0 0.0 0.02" />
<xacro:carema_xacro xyz="0.10 0.0 0.075" />
<xacro:laser_xacro xyz="0.0 0.0 0.10" />

<xacro:wheel_xacro wheel_name="left_wheel" xyz="0 0.10 -0.06" />
<xacro:wheel_xacro wheel_name="right_wheel" xyz="0 -0.10 -0.06" />
<xacro:caster_xacro caster_name="front_wheel" xyz="0.08 0.0 -0.076" />
<xacro:caster_xacro caster_name="back_wheel" xyz="-0.08 0.0 -0.076" />

</robot>

6.2.6 贴合地面，添加虚拟部件

为机器人添加 base_footprint

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="base_xacro" params="length radius">
        <link name="base_footprint" />

        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
        </link>

        <joint name="joint_name" type="fixed">
            <origin xyz="0.0 0.0 ${length/2.0 + 0.0325 - 0.001}" rpy="0.0 0.0 0.0"/>
            <parent link="base_footprint"/>
            <child link="base_link"/>
        </joint>
    </xacro:macro>
</robot>

6.3 机器人的物理属性

6.3.1 为机器人添加碰撞属性

在机器人模型下方添加collision标签，内容可以和visual标签保持一致

        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </collision>

在这里插入图片描述
可以将 rviz2 的标签修改为 collision 可见验证

6.3.2 为机器人部件添加质量和惯性

惯性矩阵

添加惯性宏文件

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
    <xacro:macro name="box_inertia" params="m l w h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(h*h + l*l)/12}" ixy="0" ixz="0" iyy="${m*(w*w + l*l)/12}" iyz="0" izz="${m*(w*w + h*h)/12}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="cylinder_inertia" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0" iyy="${m*(3*r*r+h*h)/12}" iyz="0" izz="${m*r*r/2}" />
        </inertial>
    </xacro:macro>

    <xacro:macro name="sphere_inertia" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0" iyy="${2*m*r*r/5}" iyz="0" izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>
</robot>

为机器人添加宏

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertial.xacro"/>
    
    <xacro:macro name="base_xacro" params="length radius">
        <link name="base_footprint" />

        <link name="base_link">
            <visual>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </visual>
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <cylinder radius="${radius}" length="${length}"/>
                </geometry>
                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5"/>
                </material>
            </collision>
            
            <xacro:cylinder_inertia m="1.0" r="${radius}" h="${length}" />
        </link>

        <joint name="joint_name" type="fixed">
            <origin xyz="0.0 0.0 ${length/2.0 + 0.0325 - 0.001}" rpy="0.0 0.0 0.0"/>
            <parent link="base_footprint"/>
            <child link="base_link"/>
        </joint>
    </xacro:macro>
</robot>

全部添加完成后，可以启动查看质量和惯性
在这里插入图片描述

6.4 Gazebo 机器人仿真

gazebo 安装

sudo apt install gazebo
mkdir -p ~/.gazebo
cd ~/.gazebo
git clone https://gitee.com/ohhuo/gazebo_models.git

修改文件夹名 gazebo_models 为 models

sudo rm -rf ~/.gazebo/models/.git
sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs

可以启动 gazebo 验证

gazebo

6.4.2 gazebo 中加载机器人模型

import launch
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os
import launch_ros.parameter_descriptions

def generate_launch_description():
    # 获取功能包的 share 路径
    urdf_package_path = get_package_share_directory('fishbot_description')

    default_xacro_path = os.path.join(urdf_package_path,'urdf','fishbot/fishbot.urdf.xacro')
    # default_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot_model.rviz')
    default_gazebo_world_path = os.path.join(urdf_package_path,'world','custor_room.world')
    # 声明一个 urdf 目录的参数，方便修改
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name="model",default_value=str(default_xacro_path),description='加载的模型文件路经'
    )
    # 通过文件路径，获取内容，并转换成参数值对象，以供传入 robot_state_publisher
    substitutions_commend_result = launch.substitutions.Command(['xacro ',launch.
    substitutions.LaunchConfiguration('model')])
    robot_description_value = launch_ros.parameter_descriptions.ParameterValue(substitutions_commend_result,
    value_type=str)

    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}]
    )
    
    action_launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']
        ),
        launch_arguments=[('world',default_gazebo_world_path),('verbose','true')]
    )

    action_spawn_entity = launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic','/robot_description','-entity','fishbot']
    )

    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity
    ])

这里因为某些标签没有转换，加载出来是白膜

6.4.3 用 gazebo 标签拓展 URDF

颜色拓展

        <gazebo reference="laser_cylinder_link" >
            <material>Gazebo/Black</material>
        </gazebo>
        <gazebo reference="laser_link" >
            <material>Gazebo/Black</material>
        </gazebo>

摩擦力

        <gazebo reference="${wheel_name}_link">
            <mu1 value="20.0"/>
            <mu2 value="20.0"/>
            <kp value="1000000000.0"/>
            <kd value="1.0"/>
        </gazebo>

--------
        <gazebo reference="${wheel_name}_link">
            <mu1 value="0.0"/>
            <mu2 value="0.0"/>
            <kp value="1000000000.0"/>
            <kd value="1.0"/>
        </gazebo>

6.4.4 使用两轮差速插件控制机器人运动

fishbot 下新建 plugins 文件夹，新建 gazebo_control_plugin.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="gazebo_control_plugin">
        <gazebo>
            <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
                <ros>
                    <namespace>/</namespace>
                    <remapping>cmd_vel:=cmd_vel</remapping>
                    <remapping>odom:=odom</remapping>
                </ros>
                <update_rate>30</update_rate>
                <!-- wheels -->
                <left_joint>left_wheel_joint</left_joint>
                <right_joint>right_wheel_joint</right_joint>
                <!-- kinematics -->
                <wheel_separation>0.2</wheel_separation>
                <wheel_diameter>0.064</wheel_diameter>
                <!-- limits -->
                <max_wheel_torque>20</max_wheel_torque>
                <max_wheel_acceleration>1.0</max_wheel_acceleration>
                <!-- output -->
                <publish_odom>true</publish_odom>
                <publish_odom_tf>true</publish_odom_tf>
                <publish_wheel_tf>true</publish_wheel_tf>

                <odometry_frame>odom</odometry_frame>
                <robot_base_frame>base_footprint</robot_base_frame>
            </plugin>
        </gazebo>
   </xacro:macro>
</robot>

fishbot

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fishbot">
<!-- 基础部分 base_link -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro"/>
<!-- 传感器部分 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/carema.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/laser.urdf.xacro"/>
<!-- 执行器部分 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/wheel.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/caster.urdf.xacro"/>
<!-- 插件 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_control_plugin.xacro"/>

<xacro:base_xacro length="0.12" radius="0.10" />
<xacro:imu_xacro xyz="0.0 0.0 0.02" />
<xacro:carema_xacro xyz="0.10 0.0 0.075" />
<xacro:laser_xacro xyz="0.0 0.0 0.10" />

<xacro:wheel_xacro wheel_name="left_wheel" xyz="0 0.10 -0.06" />
<xacro:wheel_xacro wheel_name="right_wheel" xyz="0 -0.10 -0.06" />
<xacro:caster_xacro caster_name="front_wheel" xyz="0.08 0.0 -0.076" />
<xacro:caster_xacro caster_name="back_wheel" xyz="-0.08 0.0 -0.076" />

<xacro:gazebo_control_plugin />

</robot>

6.4.5 激光雷达传感器仿真

        <gazebo reference="laser_link">
            <sensor name="laserscan" type="ray">
                <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
                    <ros>
                        <namespace>/</namespace>
                        <remapping>~/out:=scan</remapping>
                    </ros>
                    <output_type>sensor_msgs/LaserScan</output_type>
                    <frame_name>laser_link</frame_name>
                </plugin>
                <always_on>true</always_on>
                <visualize>true</visualize>
                <update_rate>5</update_rate>
                <pose>0 0 0 0 0 0</pose>
								<!-- 激光传感器配置 -->
                <ray>
                    <!-- 设置扫描范围 -->
                    <scan>
                        <horizontal>
                            <samples>360</samples>
                            <resolution>1.000000</resolution>
                            <min_angle>0.000000</min_angle>
                            <max_angle>6.280000</max_angle>
                        </horizontal>
                    </scan>
                    <!-- 设置扫描距离 -->
                    <range>
                        <min>0.120000</min>
                        <max>8.0</max>
                        <resolution>0.015000</resolution>
                    </range>
                    <!-- 设置噪声 -->
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                    </noise>
                </ray>
            </sensor>
        </gazebo>

6.4.6 惯性测量传感器仿真

        <gazebo reference="imu_link">
            <sensor name="imu_sensor" type="imu">
                <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
                    <ros>
                        <namespace>/</namespace>
                        <remapping>~/out:=imu</remapping>
                    </ros>
                    <initial_orientation_as_reference>false</initial_orientation_as_reference>
                </plugin>
                <update_rate>100</update_rate>
                <always_on>true</always_on>
                <!-- 六轴噪声设置 -->
                <imu>
                    <angular_velocity>
                        <x>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>2e-4</stddev>
                                <bias_mean>0.0000075</bias_mean>
                                <bias_stddev>0.0000008</bias_stddev>
                            </noise>
                        </x>
                        <y>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>2e-4</stddev>
                                <bias_mean>0.0000075</bias_mean>
                                <bias_stddev>0.0000008</bias_stddev>
                            </noise>
                        </y>
                        <z>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>2e-4</stddev>
                                <bias_mean>0.0000075</bias_mean>
                                <bias_stddev>0.0000008</bias_stddev>
                            </noise>
                        </z>
                    </angular_velocity>
                    <linear_acceleration>
                        <x>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>1.7e-2</stddev>
                                <bias_mean>0.1</bias_mean>
                                <bias_stddev>0.001</bias_stddev>
                            </noise>
                        </x>
                        <y>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>1.7e-2</stddev>
                                <bias_mean>0.1</bias_mean>
                                <bias_stddev>0.001</bias_stddev>
                            </noise>
                        </y>
                        <z>
                            <noise type="gaussian">
                                <mean>0.0</mean>
                                <stddev>1.7e-2</stddev>
                                <bias_mean>0.1</bias_mean>
                                <bias_stddev>0.001</bias_stddev>
                            </noise>
                        </z>
                    </linear_acceleration>
                </imu>
            </sensor>
        </gazebo>

6.4.7 深度相机传感器仿真

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
   <xacro:include filename="$(find fishbot_description)/urdf/fishbot/common_inertial.xacro" />
    <xacro:macro name="camera_xacro" params="xyz">
        <!-- ============相机模块================ -->
        <link name="camera_link">
            <visual>
                <origin xyz="0 0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.02 0.10 0.02" />
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.8"/>
                </material>
            </visual>
            <collision>
                <origin xyz="0 0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.02 0.10 0.02" />
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.8"/>
                </material>
            </collision>
            <xacro:box_inertia m="0.01" l="0.02" w="0.02" h="0.10" />
        </link>
        <link name="camera_optical_link"></link>
        <joint name="camera_optical_joint" type="fixed">
            <parent link="camera_link" />
            <child link="camera_optical_link" />
            <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
        </joint>

        <joint name="camera_joint" type="fixed">
            <parent link="base_link" />
            <child link="camera_link" />
            <origin xyz="${xyz}" />
        </joint>
    </xacro:macro>

</robot>

    <gazebo reference="camera_link">
        <sensor type="depth" name="camera_sensor">
            <plugin name="depth_camera" filename="libgazebo_ros_camera.so">
                <frame_name>camera_optical_link</frame_name>
            </plugin>
            <always_on>true</always_on>
            <update_rate>10</update_rate>
            <camera name="camera">
                <horizontal_fov>1.5009831567</horizontal_fov>
                <image>
                    <width>800</width>
                    <height>600</height>
                    <format>R8G8B8</format>
                </image>
                <distortion>
                    <k1>0.0</k1>
                    <k2>0.0</k2>
                    <k3>0.0</k3>
                    <p1>0.0</p1>
                    <p2>0.0</p2>
                    <center>0.5 0.5</center>
                </distortion>
            </camera>
        </sensor>
    </gazebo>

6.5 使用 ROS2_control 驱动机器人

安装 ros2_control

sudo apt install ros-$ROS_DISTRO-ros2-control
sudo apt install ros-$ROS_DISTRO-ros2-controllers
sudo apt install ros-$ROS_DISTRO-gazebo-ros2-control

6.5.2 Gazebo 接入 ros2_control

config 下新建 fishbot_ros2_control.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="fishbot_ros2_control">
    <ros2_control name="FishBotGazeboSystem" type="system">
            <hardware>
                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
            </hardware>
            <joint name="left_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-1</param>
                    <param name="max">1</param>
                </command_interface>
                <command_interface name="effort">
                    <param name="min">-0.1</param>
                    <param name="max">0.1</param>
                </command_interface>
                <state_interface name="position" />
                <state_interface name="velocity" />
                <state_interface name="effort" />
            </joint>
            <joint name="right_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-1</param>
                    <param name="max">1</param>
                </command_interface>
                <command_interface name="effort">
                    <param name="min">-0.1</param>
                    <param name="max">0.1</param>
                </command_interface>
                <state_interface name="position" />
                <state_interface name="velocity" />
                <state_interface name="effort" />
            </joint>
        </ros2_control>
        <gazebo>
        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
            <parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
        </plugin>
</gazebo>
    </xacro:macro>
</robot>

修改 fishbot.urdf.xacro
include fishbot.ros2_control.xacro
加入宏 fishbot_ros2_control

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fishbot">
<!-- 基础部分 base_link -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro"/>
<!-- 传感器部分 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/camera.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/laser.urdf.xacro"/>
<!-- 执行器部分 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/wheel.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/actuator/caster.urdf.xacro"/>
<!-- 插件 -->
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_control_plugin.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/fishbot.ros2_control.xacro"/>

<xacro:base_xacro length="0.12" radius="0.10" />
<xacro:imu_xacro xyz="0.0 0.0 0.02" />
<xacro:camera_xacro xyz="0.10 0.0 0.075" />
<xacro:laser_xacro xyz="0.0 0.0 0.10" />

<xacro:wheel_xacro wheel_name="left_wheel" xyz="0 0.10 -0.06" />
<xacro:wheel_xacro wheel_name="right_wheel" xyz="0 -0.10 -0.06" />
<xacro:caster_xacro caster_name="front_wheel" xyz="0.08 0.0 -0.076" />
<xacro:caster_xacro caster_name="back_wheel" xyz="-0.08 0.0 -0.076" />

<!-- <xacro:gazebo_control_plugin /> -->
<xacro:gazebo_sensor_plugin />
<xacro:fishbot_ros2_control />

</robot>

列出所有控制器的类型

ros2 control list_controller_types

硬件接口

ros2 control list_hardware_interfaces

硬件组件

ros2 control list_hardware_components

6.5.3 使用关节状态发布控制器

此时打开 rviz2 会发现左右轮错误

yaml 增加

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    use_sim_time: true

    fishbot_joint_state_broadcaster:
     type: joint_state_broadcaster/JointStateBroadcaster

初始化并激活 fishbot_joint_state_broadcaster 控制器

ros2 control load_controller fishbot_joint_state_broadcaster --set-state active

将激活加入 launch

    action_load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=["ros2", "control", "load_controller", "fishbot_joint_state_broadcaster", "--set-state", "active"],
        output='screen'
    )

.......
    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[action_load_joint_state_controller],
            )
        )
    ])

6.5.4 使用力控制器控制轮子

yaml 文件增加力控制器

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    use_sim_time: true

    fishbot_joint_state_broadcaster:
     type: joint_state_broadcaster/JointStateBroadcaster
    fishbot_effort_controller:
      type: effort_controllers/JointGroupEffortController

fishbot_effort_controller:
  ros__parameters:
    joints:
      - left_wheel_joint
      - right_wheel_joint
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

launch

    action_load_effort_controller = launch.actions.ExecuteProcess(
        cmd=["ros2", "control", "load_controller", "fishbot_effort_controller", "--set-state", "active"],
        output='screen'
    )

    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[action_load_joint_state_controller],
            )
        ),
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_state_controller,
                on_exit=[action_load_effort_controller],
            )
        )
    ])

6.5.5 用差速控制器控制机器人

yaml 加入差速控制器

    fishbot_diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

fishbot_diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]

    wheel_separation: 0.17
    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
    wheel_radius: 0.032

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_footprint
    pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
    twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]

    open_loop: true
    enable_odom_tf: true

    cmd_vel_timeout: 0.5
    #publish_limited_velocity: true
    use_stamped_vel: false
    #velocity_rolling_window_size: 10

删除力控制器，因为力控制器也能驱动车轮转动

    action_load_diff_driver_controller = launch.actions.ExecuteProcess(
        cmd=["ros2", "control", "load_controller", "fishbot_diff_drive_controller", "--set-state", "active"],
        output='screen'
    )
    # action_load_diff_drive_controller = launch.actions.ExecuteProcess(
    #     cmd=["ros2", "control", "load_controller", "fishbot_diff_drive_controller", "--set-state", "active"],
    #     output='screen'
    # )

    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[action_load_joint_state_controller],
            )
        ),
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_state_controller,
                on_exit=[action_load_diff_driver_controller],
            )
        ),
        # launch.actions.RegisterEventHandler(
        #     event_handler=launch.event_handlers.OnProcessExit(
        #         target_action=action_load_joint_state_controller,
        #         on_exit=[action_load_diff_drive_controller],
        #     )
        # )
    ])

重映射（实际上已经不需要了）
将 remap 添加到控制器下

        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
            <parameters>$(find fishbot_description)/config/fishbot_ros2_controller.yaml</parameters>
            <ros>
                <remapping>//fishbot_diff_drive_controller/odom:=/odom</remapping>
                <remapping>//fishbot_diff_drive_controller/cmd_vel:=/cmd_vel</remapping>
            </ros>
        </plugin>