鱼香ROS2学习 第六、七章 仿真与导航

名词注释

<link>            <!-- 机器人身体部分 -->
<visual>          <!-- 部件外观描述 --> 
<origin>          <!-- 沿自己几何中心的偏移与旋转量 -->   
<geometry>        <!-- 几何形状 -->
<material>        <!-- 材质子标签 -->    
<collision>       <!-- 碰撞属性 -->
<inertial>        <!-- 质量/惯性属性 -->
</plugin>         <!--    插件    -->
                   
            

URDF

URDF使用XML(Extensible Markup Language，可扩展标记语言)来描述机器人的几何结构、传感器和执行器等信息。
 

<?xml version="1.0"?>
<robot name="first robot">
    <!-- XML 注释 -->
    <link name="base link"></link>
</robot>

一个UFDF的例子：first_robot.urdf

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

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

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

</robot>

使用Xacro简化URDF文件

Xacro(XML Macro)是基于XML的宏语言，用于简化URDF文件的创建和维护。使用它可以将部件等定义为宏，在需要使用的时候进行调用即可。
 

<robot lxmlns:xacro="http://www.ros.org/wiki/xacro"name="first robot">
<!-- 声明 base 模块 -->
    <xacro:macro name="base link"params="length radius">
        <link name="base link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${length}" radius="${length}" />
                </geometry>

                <material name="white">
                    <color rgba="1.0 1.0 1.0 0.5" />
                </material>
            </visual>
        </link>
    </xacro:macro>
<!--传递参数调用 base link 模块 -->
<xacro:base link length="0.12" radius="0.1" />

仿真软件Gazebo

Gazebo使用的是sdf格式，而我们的机器人建模使用的是URDF，所以要想在Gazebo中显示机器人模型，就需要将URDF转换成sdf。不用操心如何转换，因为ROS2提供了一些功能包，可以帮助我们直接实现这一转换。

ros2将URDF模型转换为sdf文件

sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs

在gazebo中显示机器人模型的launch文件

#在gazebo中显示机器人模型的launch文件
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    # 获取功能包的默认路径
    robot_name_in_model = "fishbot"
    urdf_tutorial_path = get_package_share_directory('fishbot_description')
    default_model_path = urdf_tutorial_path + '/urdf/fishbot/fishbot.urdf.xacro'
    default_world_path = urdf_tutorial_path + '/world/custom_room.world'
    # 为 Launch 声明参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', default_value=str(default_model_path),description='URDF 的绝对路径')
    # 获取文件内容生成新的参数
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        launch.substitutions.Command(
            ['xacro ', launch.substitutions.LaunchConfiguration('model')]),
        value_type=str)
  	
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}]
    )

    # 通过 IncludeLaunchDescription 包含另外一个 launch 文件
    launch_gazebo = launch.actions.IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory(
            'gazebo_ros'), '/launch', '/gazebo.launch.py']),
      	# 传递参数
        launch_arguments=[('world', default_world_path),('verbose','true')]
    )
    # 请求 Gazebo 加载机器人
    spawn_entity_node = launch_ros.actions.Node(
        package='gazebo_ros',#功能包名字
        executable='spawn_entity.py',#可执行文件名字
        arguments=['-topic', '/robot_description',#依赖
                   '-entity', robot_name_in_model, ])
    
    # 加载并激活 fishbot_joint_state_broadcaster 控制器
    load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
            'fishbot_joint_state_broadcaster'],
        output='screen'
    )

    # 加载并激活 fishbot_effort_controller 控制器
    load_fishbot_effort_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_effort_controller'], 
        output='screen')
    
    load_fishbot_diff_drive_controller = launch.actions.ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_diff_drive_controller'], 
        output='screen')
    
    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        robot_state_publisher_node,
        launch_gazebo,
        spawn_entity_node,
        # 事件动作，当加载机器人结束后执行    
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=spawn_entity_node,
                on_exit=[load_joint_state_controller],)
            ),
        # 事件动作，load_fishbot_diff_drive_controller
        launch.actions.RegisterEventHandler(
        event_handler=launch.event_handlers.OnProcessExit(
            target_action=load_joint_state_controller,
            on_exit=[load_fishbot_diff_drive_controller],)
            ),
    ])

 使用gazebo标签扩展URDF

<xacro:macro name="laser_xacro" params="xyz">
    <gazebo reference="laser_cylinder link">
        <material>Gazebo/Black</material>
    </gazebo>

    <gazebo reference="laser link">
        <material>Gazebo/Black</material>
    </gazebo>
    ···
</xacro:macro>

使用gazebo插件 （宏）

#gazebo插件格式：
<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>
    </plugin>
</gazebo>

#以下是一个  使用两轮差速插件控制机器人 实例：
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="gazebo_control_plugin">
        <gazebo>
            <!-- 插件名，插件文件名 -->   <!-- 这里用了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>

 ros2_control介绍与安装

gazebo插件模式架构

 ros2_control模式架构

安装代码

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

Gazebo接入ros2_control

Gazebo接入ros2_control，其实就是让Gazebo按照rcs2_control指定的接口提供数据。在ROS 2中利用相应的Gazebo插件，可以方便的实现Gazebo和ros2_control的对接。

安装代码

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

 ros2_control的宏（使用gazebo 介入ros2_control）

<!--    使用gazebo 介入ros2_control    -->
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--    定义了一个fishbot_ros2_control 宏，标签ros2_control 描述硬件资源。    -->
    <xacro:macro name="fishbot_ros2_control">
    <ros2_control name="FishBotGazeboSystem" type="system">
            <!--    驱动库    -->
            <hardware>
                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
            </hardware>
            <!--    下面的关节joint,定义了左右两个轮子，有控制接口跟状态接口。    -->
            <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标签定义了用来解析ros2_control的标签的插件。里面的参数需要config目录新建文件：fishbot_ros2_controller.yaml    -->
        <gazebo>
        <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/cmd_vel_unstamped:=/cmd_vel</remapping><!-- 重映射  -->
                <remapping>/fishbot_diff_drive_controller/odom:=/odom</remapping>
            </ros>
        </plugin>
</gazebo>
    </xacro:macro>
</robot>

ros2 control 命令

#加载控制器
ros2 control load_controller --set-state configure fishbot_diff_drive_controller

#启动已加载的控制器
ros2 control set_controller_state fishbot_diff_drive_controller start

#一次性加载并启动控制器
ros2 control load_start_controller fishbot_diff_drive_controller

导航

构建第一张导航地图

sudo apt install ros-$ROS_DISTRO-slam-toolbox

ros2  launch slam_toolbox online_async_launch.py use_sim_time:=True

 将地图保存为文件

room.yaml

image: room.pgm
mode: trinary                #类型
resolution: 0.05             #分辨率  5cm   
origin: [-10.4, -6.53, 0]    #地图原点
negate: 0                    #是否对地图进行取反 0表示不取反
occupied_thresh: 0.65        #占据，自由，未知之间的分界线        
free_thresh: 0.25            #占据，自由，未知之间的分界线 

 机器人导航框架Navigation 2

编写launch并启动导航

# 导入操作系统接口库（用于文件路径处理）
import os

# 导入ROS 2核心启动模块（定义节点、参数等基础功能）
import launch

# 导入ROS 2节点操作模块（包含Node、LifecycleNode等关键类）
import launch_ros

# 从ament索引工具获取ROS 2包的资源路径（如package.xml所在目录）
from ament_index_python.packages import get_package_share_directory

# 支持包含其他Python格式的launch文件（用于模块化启动设计）
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    # 获取与拼接默认路径
    fishbot_navigation2_dir = get_package_share_directory(
        'fishbot_navigation2')# 获取功能包的路径
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(
        nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
    
    # 创建 Launch 配置
    use_sim_time = launch.substitutions.LaunchConfiguration(
        'use_sim_time', default='true')#是否使用仿真时间
    map_yaml_path = launch.substitutions.LaunchConfiguration(
        'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml'))
    nav2_param_path = launch.substitutions.LaunchConfiguration(
        'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml'))

    return launch.LaunchDescription([
        # 声明新的 Launch 参数
        # 声明ROS 2启动参数：仿真时间开关 # 作用：控制节点是否使用仿真时间（如Gazebo/Webots等），而非系统实时时钟
        launch.actions.DeclareLaunchArgument(
            'use_sim_time',                                     # 参数名称（需与节点参数名一致）
            default_value=use_sim_time,                         # 默认值应为字符串（ROS 2自动转换类型）
            description='Use simulation (Gazebo) clock if true' # 参数描述（命令行中通过-h查看）
            ),
        launch.actions.DeclareLaunchArgument(
            'map',                      # 参数名称（需与节点参数名一致）
            default_value=map_yaml_path,# 导航参数文件默认路径
            description='地图文件完整路径' # 参数用途说明
        ),
        launch.actions.DeclareLaunchArgument(
            'params_file', 
            default_value=nav2_param_path,# 导航参数文件默认路径
            description='参数文件完整路径'   # 参数用途说明
        ),
        # 包含Nav2的启动文件
        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [nav2_bringup_dir, '/launch', '/bringup_launch.py'# Nav2主启动文件路径
            ]),
            # 使用 Launch 参数替换原有参数
            launch_arguments={
                'map': map_yaml_path,# 覆盖地图路径参数
                'use_sim_time': use_sim_time,# 仿真时间设置
                'params_file': nav2_param_path# 覆盖参数文件路径
            }.items(),# 将字典转换为键值对列表
        ),
        # 启动RViz2可视化工具
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2', # 节点名称
            arguments=['-d', rviz_config_dir],# 指定RViz配置文件
            parameters=[{'use_sim_time': use_sim_time}],# 传递仿真时间参数
            output='screen'),# 输出显示在控制台
    ])