基于 ROS 机器人和 RTAB-MAP 算法实现室内三维重建
本文叙如何利用RTAB-Map算法和Turtlebot3机器人在自己构建的室内场景中建图
文章目录
-
-
- 1、安装依赖
- 2、创建工作空间
- 3、安装rtabmap和rtabmap_ros
- 4、建立gazebo场景功能包
- 5、建立机器人功能包
- 6、为机器人添加kinect相机参考
- 7、编译工作空间
- 8、建立环境地图
- 9、建图
-
1、安装依赖
必要的依赖安装/卸载 (Qt, PCL, VTK, OpenCV, …)
sudo apt-get install ros-kinetic-rtabmap ros-kinetic-rtabmap-ros
sudo apt-get remove ros-kinetic-rtabmap ros-kinetic-rtabmap-ros
2、创建工作空间
#创建rtabmap_ws，在rtabmap_ws下
mkdir src
cd src
catkin_init_workspace
cd..
catkin_make
echo "source ~/catkin_ws_rtab/devel/setup.bash " >> ~/.bashrc
source ~/.bashrc
3、安装rtabmap和rtabmap_ros
#任意目录下
git clone https://github.com/introlab/rtabmap.git rtabmap
cd rtabmap/build
cmake -DCMAKE_INSTALL_PREFIX=~/catkin_ws/devel ..
make -j4
make install
#在 rtabmap_ws下
git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
catkin_make -j1
4、建立gazebo场景功能包
（1）在 rtabmap_ws 下创建功能包 rtab_room
catkin_create_pkg rtab_room std_msgs rospy roscpp
cd ..
catkin_make
source devel/setup.bash
（2）在rtab_room功能包下放置worlds文件夹，其中包含之前搭建好的demo02.world文件
搭建方法参考：Gazebo仿真环境搭建
（3）编写launch文件
在rtab_room功能包下建立launch文件夹，并在launch文件夹中新建rtab_room.launch文件，内容如下：
<launch>
<!-- 设置launch文件的参数 -->
<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"/>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find rtab_room)/world/my_room.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
<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="turtlebot3_description" command="$(find xacro)/xacro --inorder '$(find turtlebot3_description)/urdf/turtlebot3_burger.urdf.xacro'" />
<!-- 在gazebo中加载机器人模型 -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model turtlebot3_burger -param turtlebot3_description"/>
</launch>
（4）编译刷新环境
catkin_make
source devel/setup.bash
5、建立机器人功能包
#rtabmap_ws/src下
git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
cd ~/rtabmap_ws
rosdep install --from-paths src -i -y
catkin_make
6、为机器人添加kinect相机参考
在Turtlebot3-Burger机器人已有激光雷达的基础上，通过修改xacro描述文件、添加模型文件，为其添加kinect深度相机并能够获取深度图像。
（1）构建kinect描述文件（kinect_gazebo.xacro）
注： kinect_gazebo.xacro 放置于 turtlebot3/turtlebot3_description/urdf 文件夹下
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera">
<xacro:macro name="kinect_camera" params="prefix:=camera">
<link name="${prefix}_link">
<origin xyz="0 0 0" rpy="0 0 0"/>
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/kinect.dae" scale="0.4 0.4 0.4" />
geometry>
visual>
<collision>
<geometry>
<box size="0.07 0.3 0.09"/>
geometry>
collision>
link>
<joint name="${prefix}_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
<parent link="${prefix}_link"/>
<child link="${prefix}_frame_optical"/>
joint>
<link name="${prefix}_frame_optical"/>
<gazebo reference="${prefix}_link">
<sensor type="depth" name="${prefix}">
<always_on>truealways_on>
<update_rate>20.0update_rate>
<camera>
<horizontal_fov>${60.0*M_PI/180.0}horizontal_fov>
<image>
<format>R8G8B8format>
<width>640width>
<height>480height>
image>
<clip>
<near>0.05near>
<far>8.0far>
clip>
camera>
<plugin name="kinect_${prefix}_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>${prefix}cameraName>
<alwaysOn>truealwaysOn>
<updateRate>10updateRate>
<imageTopicName>rgb/image_rawimageTopicName>
<depthImageTopicName>depth/image_rawdepthImageTopicName>
<pointCloudTopicName>depth/pointspointCloudTopicName>
<cameraInfoTopicName>rgb/camera_infocameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_infodepthImageCameraInfoTopicName>
<frameName>${prefix}_frame_opticalframeName>
<baseline>0.1baseline>
<distortion_k1>0.0distortion_k1>
<distortion_k2>0.0distortion_k2>
<distortion_k3>0.0distortion_k3>
<distortion_t1>0.0distortion_t1>
<distortion_t2>0.0distortion_t2>
<pointCloudCutoff>0.4pointCloudCutoff>
plugin>
sensor>
gazebo>
xacro:macro>
robot>
（2）修改turtlebot3_burger.urdf.xacro文件
注： turtlebot3_burger.urdf.xacro 文件放置于 turtlebot3_description/urdf 文件夹下，替换原有同名文件。代码中5-17行为添加内容。
<robot name="turtlebot3_burger" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.xacro"/>
<xacro:include filename="$(find turtlebot3_description)/urdf/turtlebot3_burger.gazebo.xacro"/>
<xacro:include filename="$(find turtlebot3_description)/urdf/kinect_gazebo.xacro"/>
<xacro:property name="kinect_offset_x" value="0.0" />
<xacro:property name="kinect_offset_y" value="0.0" />
<xacro:property name="kinect_offset_z" value="0.1" />
<xacro:property name="M_PI" value="3.14159" />
<xacro:kinect_camera prefix="camera"/>
<joint name="kinect_frame_joint" type="fixed">
<origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="camera_link"/>
joint>
<link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0.0 0.0 0.010" rpy="0 0 0"/>
joint>
<link name="base_link">
<visual>
<origin xyz="-0.032 0 0.0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/bases/burger_base.stl" scale="0.001 0.001 0.001"/>
geometry>
<material name="light_black"/>
visual>
<collision>
<origin xyz="-0.032 0 0.070" rpy="0 0 0"/>
<geometry>
<box size="0.140 0.140 0.143"/>
geometry>
collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="8.2573504e-01"/>
<inertia ixx="2.2124416e-03" ixy="-1.2294101e-05" ixz="3.4938785e-05"
iyy="2.1193702e-03" iyz="-5.0120904e-06"
izz="2.0064271e-03" />
inertial>
link>
<joint name="wheel_left_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_left_link"/>
<origin xyz="0.0 0.08 0.023" rpy="-1.57 0 0"/>
<axis xyz="0 0 1"/>
joint>
<link name="wheel_left_link">
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
geometry>
<material name="dark"/>
visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.018" radius="0.033"/>
geometry>
collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="2.8498940e-02" />
<inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
iyy="1.1192413e-05" iyz="-1.4400107e-11"
izz="2.0712558e-05" />
inertial>
link>
<joint name="wheel_right_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_right_link"/>
<origin xyz="0.0 -0.080 0.023" rpy="-1.57 0 0"/>
<axis xyz="0 0 1"/>
joint>
<link name="wheel_right_link">
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
geometry>
<material name="dark"/>
visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.018" radius="0.033"/>
geometry>
collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="2.8498940e-02" />
<inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
iyy="1.1192413e-05" iyz="-1.4400107e-11"
izz="2.0712558e-05" />
inertial>
link>
<joint name="caster_back_joint" type="fixed">
<parent link="base_link"/>
<child link="caster_back_link"/>
<origin xyz="-0.081 0 -0.004" rpy="-1.57 0 0"/>
joint>
<link name="caster_back_link">
<collision>
<origin xyz="0 0.001 0" rpy="0 0 0"/>
<geometry>
<box size="0.030 0.009 0.020"/>
geometry>
collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="0.005" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
inertial>
link>
<joint name="imu_joint" type="fixed">
<parent link="base_link"/>
<child link="imu_link"/>
<origin xyz="-0.032 0 0.068" rpy="0 0 0"/>
joint>
<link name="imu_link"/>
<joint name="scan_joint" type="fixed">
<parent link="base_link"/>
<child link="base_scan"/>
<origin xyz="-0.032 0 0.172" rpy="0 0 0"/>
joint>
<link name="base_scan">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/>
geometry>
<material name="dark"/>
visual>
<collision>
<origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
<geometry>
<cylinder length="0.0315" radius="0.055"/>
geometry>
collision>
<inertial>
<mass value="0.114" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
inertial>
link>
robot>
（3）添加kinect相机模型文件
链接：https://pan.baidu.com/s/18Z8IqXy4N8LGrvYmiLuknQ
提取码：wuat
将以上三个文件放置于turtlebot3_description/meshes文件夹下
（4）编译刷新运行，查看效果
在catkin_ws_turtlebot3工作空间下
catkin_make
source devel/setup.bash
roslaunch turtlebot3_gazebo turtlebot3_world.launch
rqt_image_view
7、编译工作空间
catkin_make
source devel/setup.bash
8、建立环境地图
#开启gazebo场景
roslaunch rtab_room rtab_room.launch
#新终端开启rtab_map算法（此处有警告，暂未解决，但是仍可以运行）
source /opt/ros/kinetic/setup.bash
roslaunch rtabmap_ros rtabmap.launch
9、建图
（1）启动gazebo场景
roslaunch rtab_room rtab_room.launch
（2）启动rtab_map算法
roslaunch rtabmap_ros demo_turtlebot_mapping.launch simulation:=true
（3）启动rviz
roslaunch rtabmap_ros demo_turtlebot_rviz.launch
（4）启动键盘控制
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
（5）建图
（6）保存地图
rosrun map_server map_saver -f ~/map