PX4 + D435i 进行gazebo仿真

为gazebo仿真添加D435i相机模块

下载realsense_gazebo_ros功能包并进行编译

mkdir -p realsense_ws/src
cd realsense_ws/src
git clone https://gitee.com/nie_xun/realsense_ros_gazebo.git
cd realsense_ws
catkin_make
source devel/setup.sh

将librealsense_gazebo_plugin.so存放到GAZEBO_PLUGIN_PATH中

查询Gazebo插件的位置

echo $GAZEBO_PLUGIN_PATH

返回的Gazebo插件的路径大致如下：
/home/username/PX4-Autopilot/build/px4_sitl_default/build_gazebo

直接将.so文件复制过去即可

librealsense_gazebo_plugin.so路径：
~/realsense_ws/devel/lib

将D435i模型安装在iris无人机上

1、查询Gazebo中模型的路径（GAZEBO_MODEL_PATH）

echo $GAZEBO_MODEL_PATH

返回的路径大致如下：
/home/username/PX4-Autopilot/Tools/sitl_gazebo/models

2、创建iris_fusion文件夹

在GAZEBO_MODEL_PATH中创建iris_fusion文件夹，在文件夹中添加iris_fusion.sdf文件和model.config文件
iris_fusion.sdf

<?xml version='1.0'?>
<sdf version='1.5'><model name='iris_D435i'><include><uri>model://iris</uri>  //引入model中的模型iris</include><include><uri>model://D435i</uri>  //引入model中的模型D435i<pose>0.12 0 0.02 1.5708 0 1.5708</pose>  //设置D435i模型的位置，分别是x y z yaw pitch roll</include><joint name="D435i_joint" type="fixed">  //创建相机关节，类型为固定关节<child>D435i::camera_link</child>  //关节的子关节为D435i的camera_link关节（camera_link关节在D435i文件夹的.sdf文件中定义）<parent>iris::base_link</parent>  //关节的父关节为iris的base_link关节<axis><xyz>0 0 0</xyz><limit><upper>0</upper><lower>0</lower></limit></axis></joint></model>
</sdf>

model.config

<?xml version="1.0"?>
<model><name>3DR Iris with D435i camera</name><version>1.0</version><sdf version='1.4'>iris_fusion.sdf</sdf><author><name>Amy Wagoner</name><email>arwagoner@gmail.com</email></author><description>This is a model of the 3DR Iris Quadrotor with an D435i camera. The original model has been created byThomas Gubler and is maintained by Lorenz Meier.</description>
</model>

3、将下图中的D435i文件夹复制到GAZEBO_MODEL_PATH

4、将前面克隆的realsense_ros_gazebo文件夹整个移动到GAZEBO_MODEL_PATH中

5、创建px4的launch文件

mavros_posix_sitl_D435i.launch

<?xml version="1.0"?>
<launch><!-- MAVROS posix SITL environment launch script --><!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle --><!-- vehicle pose --><arg name="x" default="0"/><arg name="y" default="0"/><arg name="z" default="0"/><arg name="R" default="0"/><arg name="P" default="0"/><arg name="Y" default="0"/><!-- vehicle model and world --><arg name="est" default="ekf2"/><arg name="vehicle" default="iris"/><arg name="my_model" default="iris_fusion"/><arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/><arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg my_model)/$(arg my_model).sdf"/><!-- gazebo configs --><arg name="gui" default="true"/><arg name="debug" default="false"/><arg name="verbose" default="false"/><arg name="paused" default="false"/><arg name="respawn_gazebo" default="false"/><!-- MAVROS configs --><arg name="fcu_url" default="udp://:14540@localhost:14557"/><arg name="respawn_mavros" default="false"/><!-- PX4 configs --><arg name="interactive" default="true"/><!-- PX4 SITL and Gazebo --><include file="$(find px4)/launch/posix_sitl.launch"><arg name="x" value="$(arg x)"/><arg name="y" value="$(arg y)"/><arg name="z" value="$(arg z)"/><arg name="R" value="$(arg R)"/><arg name="P" value="$(arg P)"/><arg name="Y" value="$(arg Y)"/><arg name="world" value="$(arg world)"/><arg name="vehicle" value="$(arg vehicle)"/><arg name="sdf" value="$(arg sdf)"/><arg name="gui" value="$(arg gui)"/><arg name="interactive" value="$(arg interactive)"/><arg name="debug" value="$(arg debug)"/><arg name="verbose" value="$(arg verbose)"/><arg name="paused" value="$(arg paused)"/><arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/></include><!-- MAVROS --><include file="$(find mavros)/launch/px4.launch"><!-- GCS link is provided by SITL --><arg name="gcs_url" value=""/><arg name="fcu_url" value="$(arg fcu_url)"/><arg name="respawn_mavros" value="$(arg respawn_mavros)"/></include>
</launch>

运行并测试

# 运行gazebo仿真
roslaunch px4 mavros_posix_sitl_D435i.launch
# 查看相机消息
rostopic list
# 查看相机画面
rqt_image_view

运行成功会出现带D435i的无人机、camera开头的消息、摄像头画面

参考文章

为Gazebo中的iris无人机添加realsense D435i相机

在PX4 gazebo仿真中加入realsense D435i（最新）