本系列文章是 2024 春季学期北航计算机学院本科生课程《软件工程》(嵌入式方向)的实验部分报告,不包含团队大作业项目内容与相关细节
任务1-环境配置与机器人模型搭建
工作空间创建
注意创建包时使用 urdf xacro
catkin_init_workspace cd .. catkin_make source ./devel/setup.bash catkin_create_pkg mrobot_description urdf xacro
|
然后创建 urdf
文件夹并进行 urdf
文件的书写。
RViz仿真显示
此时创建的 urdf
单个文件已经可以配合 launch
文件运行在 RViz 应用中了。
<?xml version="1.0" ?> <robot name="base">
<link name="base_link"> <visual> <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/> <geometry> <cylinder radius="1" length="0.025"/> </geometry> <material name="yellow"> <color rgba="1 0.4 0.0 1"/> </material> </visual> <inertial> <mass value="2"/> <origin xyz="0 0 0.0" /> <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.0"/> </inertial> <collision> <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/> <geometry> <cylinder radius="1" length="0.025"/> </geometry> </collision> </link>
<joint name="left_motor_base_joint" type="fixed"> <origin xyz="0.8 0.4 0" rpy="0 0 0" /> <parent link="base_link"/> <child link="left_motor" /> </joint> <link name="left_motor"> <visual> <origin xyz="0 0 0" rpy="1.5707 0 0" /> <geometry> <cylinder radius="0.1" length = "0.6"/> </geometry> <material name="gray"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin xyz="0 0 0" rpy="1.5707 0 0" /> <geometry> <cylinder radius="0.1" length = "0.6"/> </geometry> </collision> </link> <joint name="left_active_wheel_motor_joint" type="continuous"> <origin xyz="0 0.3 0.0" rpy="0.0 0.0 0.0"/> <parent link="left_motor"/> <child link="left_active_wheel_link"/> <axis xyz="0 1 0"/> </joint> <link name="left_active_wheel_link"> <visual> <origin xyz="0 0 0" rpy="1.5707 0.0 0.0"/> <geometry> <cylinder radius="0.233" length="0.017"/> </geometry> <material name="white"> <color rgba="1 1 1 0.9"/> </material> </visual> <collision> <origin xyz="0 0 0" rpy="1.5707 0.0 0.0"/> <geometry> <cylinder radius="0.233" length="0.017"/> </geometry> </collision> </link>
<joint name="right_motor_base_joint" type="fixed"> <origin xyz="0.8 -0.4 0" rpy="0 0 0" /> <parent link="base_link"/> <child link="right_motor" /> </joint> <link name="right_motor"> <visual> <origin xyz="0 0 0" rpy="1.5707 0 0" /> <geometry> <cylinder radius="0.1" length = "0.6"/> </geometry> <material name="gray"> <color rgba="0 0 0 1"/> </material> </visual> <collision> <origin xyz="0 0 0" rpy="1.5707 0 0" /> <geometry> <cylinder radius="0.1" length = "0.6"/> </geometry> </collision> </link> <joint name="right_active_wheel_motor_joint" type="continuous"> <origin xyz="0 -0.3 0.0" rpy="0.0 0.0 0.0"/> <parent link="right_motor"/> <child link="right_active_wheel_link"/> <axis xyz="0 1 0"/> </joint> <link name="right_active_wheel_link"> <visual> <origin xyz="0 0 0" rpy="1.5707 0.0 0.0"/> <geometry> <cylinder radius="0.233" length="0.017"/> </geometry> <material name="white"> <color rgba="1 1 1 0.9"/> </material> </visual> <collision> <origin xyz="0 0 0" rpy="1.5707 0.0 0.0"/> <geometry> <cylinder radius="0.233" length="0.017"/> </geometry> </collision> </link>
<joint name="left_castor_wheel_base_joint" type="fixed"> <origin xyz="-0.8 0.5 -0.1" rpy="1.5707 0.0 0.0"/> <parent link="base_link"/> <child link="left_castor_wheel_link"/> </joint> <link name="left_castor_wheel_link"> <visual> <origin xyz="0 0 0" rpy="1.5707 0.0 0.0"/> <geometry> <sphere radius="0.133"/> </geometry> <material name="white"> <color rgba="1 1 1 0.9"/> </material> </visual> <collision> <origin xyz="0 0 0" rpy="1.5707 0.0 0.0"/> <geometry> <sphere radius="0.133"/> </geometry> </collision> </link>
<joint name="right_castor_wheel_base_joint" type="fixed"> <origin xyz="-0.8 -0.5 -0.1" rpy="1.5707 0.0 0.0"/> <parent link="base_link"/> <child link="right_castor_wheel_link"/> </joint> <link name="right_castor_wheel_link"> <visual> <origin xyz="0 0 0" rpy="1.5707 0.0 0.0"/> <geometry> <sphere radius="0.133"/> </geometry> <material name="white"> <color rgba="1 1 1 0.9"/> </material> </visual> </link> </robot>
|
<launch> <param name="robot_description" textfile="$(find mrobot_description)/urdf/base.urdf" />
<param name="use_gui" value="true"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mrobot_description)/config/mrobot_urdf.rviz" required="true" /> </launch>
|
编写完毕后可以使用 roslaunch mrobot_description urdf_launcher.launch
命令行执行启动 NViz
注意到启动 NViz 后并不能正确选择 Fixed Frame(如下图),机器人也不能正常显示,搜索后需要对 joint-state
和 robot-state
进行安装,以发布正确的 tf 数据,指导 NViz 正确显示关节信息等。
sudo apt-get install ros-melodic-joint-state-publisher sudo apt-get install ros-melodic-robot-state-publisher
|
修复后可以正常启动文件类型为 urdf
的机器人
xacro项目优化
在此基础上,可以对 urdf
文件进行优化,将其模块化为 xacro
文件并进行引用,形成更复杂、层次化的工程项目,在修改项目文件时同时需要注意引用命名空间 xmlns:xacro="http://www.ros.org/wiki/xacro"
,此外也需要修改启动文件:
项目文件结构图,顶层文件为 base.urdf.xacro,引用了 base_robot.urdf.xacro、camera.urdf.xacro、rplidar.urdf.xacro
<launch> <arg name="model" default="$(find xacro)/xacro --inorder '$(find mrobot_description)/urdf/base.urdf.xacro'" /> <param name="robot_description" command="$(arg model)" /> <param name="use_gui" value="true"/> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mrobot_description)/config/mrobot_urdf.rviz" required="true" /> </launch>
|
Gazebo仿真显示
最后将机器人进行调整,实现 Gazebo 软件上的模拟,然后利用 gazebo 生成真实世界场景 world
文件。过程中同样需要修改启动文件:
<launch> <arg name="world_name" value="$(find mrobot_description)/worlds/playground.world"/> <arg name="paused" default="false"/> <arg name="use_sim_time" default="true"/> <arg name="gui" default="true"/> <arg name="headless" default="false"/> <arg name="debug" default="false"/> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" value="$(arg world_name)" /> <arg name="debug" value="$(arg debug)" /> <arg name="gui" value="$(arg gui)" /> <arg name="paused" value="$(arg paused)"/> <arg name="use_sim_time" value="$(arg use_sim_time)"/> <arg name="headless" value="$(arg headless)"/> </include> <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_description)/urdf/gazebo.urdf.xacro'" /> <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" > <param name="publish_frequency" type="double" value="50.0" /> </node> <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model mrobot -param robot_description"/> </launch>
|
在使用命令行运行时,出现了以下错误,随即 gazebo 软件崩溃:
VMware: vmw_ioctl_command error Invalid argument.
|
解决方案是关闭虚拟机的 3D 硬件加速功能。顺带一提,在 20.04 的虚拟机系统上并未出现崩溃问题。
最终的渲染效果如下: