《ROS2 机器人开发从入门道实践》鱼香ROS2—

第6章建模与仿真-创建自己的机器人

6.1 机器人建模与仿真概述

6.2使用URDF创建机器人

6.2.1 帮机器人创建一个身体

1. 新建文件chapt6/chapt6_ws/src/fishbot_description/urdf/

2. 新建文件first_robot.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><!-- 机器人IMU部件，惯性测量单元 --><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>

3. 在urdf目录下打开终端，执行urdf_to_graphviz first_robot.urdf，将会生成配置文件first_robot.gv和Created file first_robot.pdf

6.2.2 在RViz中显示机器人

1. 打开RViz，终端输入rviz2，点击add选择RobotMode，修改Description Source为File，从Description File中选择6.2.1中生成的first_robot.gv

2. 有报错，将Global Options的Fixed Frame改为base_link。还有IMU报错，需要通过消息发布建立部件之间的联系

3. 安装2个插件

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

4. 新建文件chapt6/chapt6_ws/src/fishbot_description/launch/display_robot.launch.py

节点之间的订阅关系：

import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os
import launch_ros.parameter_descriptionsdef generate_launch_description():# 获取功能包安装目录urdf_package_path = get_package_share_directory("fishbot_description")# 拼接出first_robot.urdf路径default_urdf_path = os.path.join(urdf_package_path,'urdf','first_robot.urdf')# 拼接出保存的RViz配置文件，用于自动设置配置default_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot_model.rviz')# 声明一个名为model的启动参数action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(name = 'model', default_value=str(default_urdf_path),description='加载的模型文件路径')# 获取URDF文件内容作为参数值。获取名为model的启动参数，Command会执行cat命令并将LaunchConfiguration获取的内容作为参数，# 通过value_type=str指定这个参数值的类型是字符串robot_description_value = launch_ros.parameter_descriptions.ParameterValue(launch.substitutions.Command(['cat ', launch.substitutions.LaunchConfiguration('model')]),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}])# 动态joint节点action_joint_state_publisher = launch_ros.actions.Node(package='joint_state_publisher',executable='joint_state_publisher',)# rviz启动节点+配置文件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])

5. source /home/dai/yuxiangros2/chapt6/chapt6_ws/install/setup.sh

6. 回到ws目录下，source install/setup.bash ，ros2 launch fishbot_description display_robot.launch.py 或在launch目录下 ros2 launch display_robot.launch.py --debug

7. 此时打开RViz会有红色报错，因为左侧最上面默认坐标系为map，需要手动修改其他已有的坐标系

8. 将RViz配置文件保存至chapt6/chapt6_ws/src/fishbot_description/config

# 添加保存出来的RViz配置文件
default_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot_model.rviz')# 启动rviz
action_rviz_node = launch_ros.actions.Node(package='rviz2',executable='rviz2',arguments=['-d', default_rviz_config_path]
)

修改CMakeLists.txt，添加config

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

8. 再次colcon build，运行ros2 launch fishbot_description display_robot.launch.py即可加载配置文件

6.2.3 使用Xacro简化URDF

1. 安装插件 sudo apt install ros-$ROS_DISTRO-xacro

2. 新建文件chapt6/chapt6_ws/src/fishbot_description/urdf/first_robot.xacro，将名字、参数都设置为变量

<?xml version="1.0"?>
<robot xmlns:xacro = "http://www.ros.org/wiki/xacro" name = "first_robot"><!-- 把base_link声明为宏 --><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>

3. 执行功能包

xacro 根目录/yuxiangros2/chapt6/chapt6_ws/src/fishbot_description/urdf/first_robot.xacro

4. 同步骤3，也可以用6.2.3中类似于cat的命令执行功能包，新建文件chapt6/chapt6_ws/src/fishbot_description/launch/display_robot_xacro.launch.py

内容复制display_robotlaunch.py，将“cat空格”改为“xacro空格”

5. 编译、source、在ws路径下终端执行

ros2 launch fishbot_description display_robot_xacro.launch.py model:=根目录/yuxiangros2/chapt6/chapt6_ws/src/fishbot_description/urdf/first_robot.xacro
# 完整路径通过first_robot.xacro右键获取

6.2.4 创建机器人及传感器部件

1. 新建文件base.urdf.xacro，用于声明机器人身体

<?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>

2. 创建sensor文件夹和文件imu.urdf.xacro

<?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>

3. sensor下新建文件camera.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro = "http://www.ros.org/wiki/xacro"><xacro:macro name = "camera_xacro" params = "xyz"><link name="camera_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="camera_joint" type="fixed"><parent link="base_link"/><child link="camera_link"/><origin xyz="${xyz}" rpy="0.0 0.0 0.0"/></joint></xacro:macro></robot>

4. sensor下新建文件laser.urdf.xacrol

<?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>

7. urdf下新建fishbot.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro = "http://www.ros.org/wiki/xacro" name = "fishbot"><xacro:include filename="$(find fishbot_description)/urdf/fishbot/base_urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/camera.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/laser.urdf.xacro"/><xacro:base_xacro length="0.12" radius="0.10"/></robot>

8. 运行功能包

xacro 根目录/yuxiangros2/chapt6/chapt6_ws/install/fishbot_description/share/fishbot_description/urdf/first_robot.xacro
#目录是install下的路径，不是原文件的路径

9. 修改传感器位置

<?xml version="1.0"?>
<robot xmlns:xacro = "http://www.ros.org/wiki/xacro" name = "fishbot"><xacro:include filename="$(find fishbot_description)/urdf/fishbot/base.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/camera.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/imu.urdf.xacro"/>
<xacro:include filename="$(find fishbot_description)/urdf/fishbot/sensor/laser.urdf.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"/>
</robot>

10. 运行

ros2 launch fishbot_description display_robot_xacro.launch.py model:=/home/dai/yuxiangros2/chapt6/chapt6_ws/install/fishbot_description/share/fishbot_description/urdf/fishbot/fishbot.urdf.xacro
# 复制了一个副本display_robot_xacro.launch.py（xacro执行命令行）
# 不是视频中的display_robot.launch.py（cat执行命令行）