PX4与仿真入门教程-dronedoc-使用 MoveIt 执行无人机的运动规划

PX4与仿真入门教程-dronedoc-使用 MoveIt 执行无人机的运动规划

说明：

介绍使用使用 MoveIt 执行无人机运动规划的方法！机器人运动规划库。

步骤：

安装依赖moveit

sudo apt install ros-melodic-moveit-*

创建 iris_moveit_config 包
为了使用 MoveIt 执行运动规划！需要创建一个包含机器人配置文件的特殊包。
创建该包的工具是 MoveIt! Setup Assistant。
这次我们将使用 Move It! Setup Assistant 为带有 RGBD 相机的 Iris 创建一个包。
您也可以从 COLLADA 文件创建包，但这次我们将从 xacro 文件创建它。

创建 xacro 文件

复制所需文件
首先，使用 RGBD 相机创建 Iris xacro 文件。
我已经在 px4 包中有一个 Iris 模型的 xacro 文件，所以复制它来创建一个新模型。
也可以包含xacro文件，所以不需要复制，但是这次我会复制它，因为原始文件中有我要编辑的部分。
会复制到px4_sim_pkg/models/iris_depth_camera/xacro目录下，所以如果目录不存在，用下面的命令创建。

roscd px4_sim_pkg
mkdir -p models/iris_depth_camera/xacro

使用以下命令复制所需的文件。

cp -r ~/src/Firmware/Tools/sitl_gazebo/models/rotors_description/* ~/catkin_ws/src/px4_sim_pkg/models/iris_depth_camera/xacro/

编辑 multirotor_base.xacro
然后修改multirotor_base.xacro的内容。
请先备份，因为您将更改内容。

cd ~/catkin_ws/src/px4_sim_pkg/models/iris_depth_camera/xacro/urdf
mv multirotor_base.xacro multirotor_base.xacro_

然后使用 sed 编辑内容。 sed命令是编辑输入文本（文件内容等）的命令，以下情况multirotor_base.xacro文件中的package://rotors_description替代为package://mavlink_sitl_gazebo/models/rotors_description/。使用文本编辑器进行编辑时执行相同操作。
在multirotor_base.xacro中，mesh的路径是以package的形式指定的package://模型名/meshes/mesh，从Gazebo使用的时候，这个格式没问题，但是使用setup_assistant，在这种情况下，你需要指定路径如package://包名/网格。使用 Gazebo 进行 PX4 SITL 仿真时使用的模型存储在 mavlink_sitl_gazebo 包中，因此进行以下更改。

sed -e 's/package:\/\/rotors_description\//package:\/\/mavlink_sitl_gazebo\/models\/rotors_description\//g' multirotor_base.xacro_ >| multirotor_base.xacro

如果您编辑成功并且不需要备份，请将其删除。

rm multirotor_base.xacro_

将 RGBD 相机添加到 Iris

在 iris_base.xacro 文件中的标记之前添加以下内容。
这是从使用 Octomap for URDF 创建 3D 地图中创建的带有 RGBD 相机的 Iris 模型的 SDF 文件中提取的 RGBD 相机定义的编辑部分。
iris_base.xacro添加以下内容

<!-- camera link -->
<link name="camera_link">
  <inertial>
    <mass value="0.01" />
    <origin xyz="0 0 0" rpy="0 0 0" />
    <inertia ixx="1.8e-7"
             ixy="0"
             ixz="0"
             iyy="2.1733e-6"
             iyz="0"
             izz="1.8e-7" />
  </inertial>
  <visual name="camera_visual">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <geometry>
      <box size="0.03 0.08 0.03" />
    </geometry>
  </visual>
</link>

<!-- depth camera attached to camera link -->
<gazebo reference="camera_link">
  <sensor name="camera" type="depth">
    <pose>0 0 0 1.57 -1.57 0</pose>
    <update_rate>20</update_rate>
    <camera>
      <horizontal_fov>1.047198</horizontal_fov>
      <image>
        <width>640</width>
        <height>320</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.4</near>
        <far>3.5</far>
      </clip>
    </camera>
    <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
      <baseline>0.2</baseline>
      <alwaysOn>true</alwaysOn>
      <!-- Keep this zero, update_rate in the parent <sensor> tag
        will control the frame rate. -->
      <updateRate>0.0</updateRate>
      <cameraName>camera_ir</cameraName>
      <imageTopicName>/camera/depth/image_raw</imageTopicName>
      <cameraInfoTopicName>/camera/depth/camera_info</cameraInfoTopicName>
      <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
      <depthImageInfoTopicName>/camera/depth/camera_info</depthImageInfoTopicName>
      <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
      <frameName>camera_link</frameName>
      <pointCloudCutoff>0.05</pointCloudCutoff>
      <distortionK1>0</distortionK1>
      <distortionK2>0</distortionK2>
      <distortionK3>0</distortionK3>
      <distortionT1>0</distortionT1>
      <distortionT2>0</distortionT2>
      <CxPrime>0</CxPrime>
      <Cx>0</Cx>
      <Cy>0</Cy>
      <focalLength>0</focalLength>
      <hackBaseline>0</hackBaseline>
    </plugin>
  </sensor>
</gazebo>

<!-- Joint -->
<joint name="camera_joint" type="revolute">
  <origin xyz="0.1 0 0" rpy="0 1.57 0" />
  <parent link="base_link" />
  <child link="camera_link" />
  <limit upper="0" lower="0" effort="0" velocity="0" />
</joint>

主要的变化是SDF标签改为URDF标签。
此外，为了在 URDF 中使用 Gazebo 插件，和标签包含在标签中，如下所示。
这是写在标签中的链接定义之外，所以你需要指定定义是针对哪个链接的，比如reference = "camera_link"。

<gazebo reference="camera_link">
  <sensor name="camera" type="depth">
    <pose>0 0 0 1.57 -1.57 0</pose>
    <update_rate>20</update_rate>
    <camera>
      <horizontal_fov>1.047198</horizontal_fov>
      <image>
        <width>640</width>
        <height>320</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.4</near>
        <far>3.5</far>
      </clip>
    </camera>
    <plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
      <baseline>0.2</baseline>
      <alwaysOn>true</alwaysOn>
      <!-- Keep this zero, update_rate in the parent <sensor> tag
        will control the frame rate. -->
      <updateRate>0.0</updateRate>
      <cameraName>camera_ir</cameraName>
      <imageTopicName>/camera/depth/image_raw</imageTopicName>
      <cameraInfoTopicName>/camera/depth/camera_info</cameraInfoTopicName>
      <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
      <depthImageInfoTopicName>/camera/depth/camera_info</depthImageInfoTopicName>
      <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
      <frameName>camera_link</frameName>
      <pointCloudCutoff>0.05</pointCloudCutoff>
      <distortionK1>0</distortionK1>
      <distortionK2>0</distortionK2>
      <distortionK3>0</distortionK3>
      <distortionT1>0</distortionT1>
      <distortionT2>0</distortionT2>
      <CxPrime>0</CxPrime>
      <Cx>0</Cx>
      <Cy>0</Cy>
      <focalLength>0</focalLength>
      <hackBaseline>0</hackBaseline>
    </plugin>
  </sensor>
</gazebo>

请参考以下链接将 SDF 文件转换为 URDF 文件格式。
参考1，参考2

开始

setup_assistant 可以使用以下命令启动。

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch moveit_setup_assistant setup_assistant.launch

效果图：

请输入图片描述

启动后，选择创建新的 Moveit 配置包。

请输入图片描述

系统会提示您选择一个文件，因此在 iris_depth_camera/xacro/urdf 文件夹下指定 iris_base.xacro。
此外，在可选的 xacro 参数中，要替换 xacro 文件中被指定的变量。对于 /home/username，请指定您的主目录的路径。

rotors_description_dir:=/home/ubuntu/dronedoc_ws/src/px4_sim_pkg/models/iris_depth_camera/xacro mavlink_udp_port:=14560 --inorder

按 Load Files，当模型显示如下图时，您就完成了。

请输入图片描述

之后，进行必要的设置。
如果要编辑现有包，请按编辑现有 Moveit 配置包并选择要编辑的包的目录。

请输入图片描述

Collision Matrix

选择左侧的自碰撞选项卡以配置碰撞矩阵。
这里为了提高处理速度，进行设置，排除不需要检查干扰的链路对。

请输入图片描述

生成碰撞矩阵按下按钮生成碰撞矩阵。
增加采样密度值将增加经过验证的链接链接对的数量和排除对的数量，但会增加处理时间。
它还增加了排除不需要排除的对的可能性。

请输入图片描述

Virtual Joints

选择 Virtual Joints 以配置虚拟关节。

请输入图片描述

虚拟关节是连接机器人基础框架和世界的虚拟关节。
按 Add Virtual Joint 添加一个虚拟关节。

请输入图片描述

数值如下。

虚拟关节名称/Virtual Joint Name
虚拟关节的名称

子链接/Child Link
机器人底座名称

父框架名称/Parent Frame Name
世界框架名称

关节类型/Joint Type
关节的类型。 既然是无人机，就选择浮动

完成后，按保存以保存您的设置。

请输入图片描述

Planning Group

设置计划组。
规划组用于对机器人部件进行分组。

请输入图片描述

按添加组添加组。保留除组名以外的默认值。
运动学求解器未设置，因为不需要计算逆运动学。
此外，OMPL Default Planner 可以稍后设置，因此将其保留为 (?) None。

请输入图片描述

选择添加关节以添加属于该组的关节。
仅选择 virtual_joint 并使用右箭头按钮将其添加到 Selected Joints。
完成后，按保存以保存您的设置。

请输入图片描述

保存设置后，显示会变成下图所示，双击链接移动到添加链接。

请输入图片描述

和以前一样，添加 base_link 并保存您的设置。

请输入图片描述

最终设置如下。

请输入图片描述

参数说明

机器人姿势/Robot Poses
Robot Poses 将特定的姿势保存为预设，但这次没有特别使用，因此无需设置。

末端执行器/End Effectors
此设置用于指定末端执行器。 这次不需要。

被动关节/Passive Joints
这是用于指定被动关节的设置。 通过指定被动关节，您可以通知规划器您无法控制这些关节。 这次没用。

3D 感知/3D Perception
3D Perception 是为 3D 传感器创建配置文件的设置。 如果您不需要 3D 传感器，则可以将其保留为 None。

请输入图片描述

这次安装了一个RGBD摄像头，会发布PointCloud2类型的数据，所以更改设置。
当您从下拉菜单中选择点云时，将显示一个文本框，如下所示，设置每个字段。
有两个字段要更改：检查这些值，因为它们是在 iris_base.xacro 文件中设置的。

点云主题/Point Cloud Topic
点云发表主题

最大范围/Max Range
传感器可以检索数据的最大距离

有关其他参数，请参阅感知管道教程。

请输入图片描述

Simulation

您可以输出用于 Gazebo 模拟的 URDF 文件。

请输入图片描述

ROS Control

ROS Control 使用 ros_control 包为机器人控制器生成配置文件。
这次我将不使用ros_control来控制无人机，所以我会跳过它。

请输入图片描述

Author Information

设置作者信息。

请输入图片描述

Configuration Files

在配置文件选项卡上，您可以保存在前面的步骤中创建的配置文件。设置保存目标路径，然后按生成包生成文件。
在您要保存的目录中创建一个新的 iris_moveit_config 文件夹并将其设置为保存目的地。配置文件直接在目标目录中生成。

请输入图片描述

你会得到如下图所示末端执行器没有设置的错误，但是这次末端执行器不存在，所以按OK。

请输入图片描述

文件已经生成。让我们移动到目标目录并检查生成的文件。

请输入图片描述

保存文件后，按 Exit Setup Assistant 退出。

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号