【运行背景】
ROS1 20.04 noetic
目录
【安装步骤】
【安装Rtabmap】
sudo apt install ros-noetic-rtabmap-*【安装pointcloud-to-laserscan】
sudo apt install ros-noetic-pointcloud-to-laserscan
【Gazebo仿真】
均在spark-slam-gazebo工作空间下
【小车URDF模型】
编写为spark1_description_rtabmap.urdf,Cartographer需要imu和pointcloud的数据，所以添加这两部分的传感器
a）添加深度摄像头
<link name="virtual_camera_link" />
<joint name="virtual_camera_joint"
type="fixed">
<origin
xyz="0 0 0.03"
rpy="0 0 0" />
<parent link="camera_Link" />
<child link="virtual_camera_link" />
</joint>
<gazebo reference="virtual_camera_link">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${60.0*3.14/180.0}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>5.0</far>
</clip>
</camera>
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>camera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>20.0</updateRate>
<imageTopicName>rgb/image_raw</imageTopicName>
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>upper_kinect_pointcloud_link</frameName>
<baseline>0.1</baseline>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.4</pointCloudCutoff>
</plugin>
</sensor>
</gazebo> b) 添加imu
<link name="imu_link">
<visual>
<origin xyz="0.0 0.0 0.0"/>
<geometry>
<cylinder length="0.07" radius="0.05"/>
</geometry>
</visual>
</link>
<joint
name="imu_joint"
type="fixed">
<origin
xyz="-0.0 0.0 0.0"
rpy="0 0 0" />
<parent
link="base_footprint" />
<child
link="imu_link" />
<axis
xyz="0 1 0" />
</joint>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>20</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>20</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>c）确保rviz上点云显示正确，添加虚拟关节，关联到摄像机的link，调整rpy值（rqy分别对应着xyz轴，遵循右手定理），将点云调整到正确位置
<link name="upper_kinect_pointcloud_link"/>
<joint name="upper_kinect_pointcloud_joint" type="fixed">
<origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
<parent link="virtual_camera_link"/>
<child link="upper_kinect_pointcloud_link"/>
</joint>将深度摄像头插件中的frameName设为新增加的link（这一部分和上述添加摄像头代码有重合部分）
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>camera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>20.0</updateRate>
<imageTopicName>rgb/image_raw</imageTopicName>
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>upper_kinect_pointcloud_link</frameName>
<baseline>0.1</baseline>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.4</pointCloudCutoff>
</plugin>【编写launch文件】
取名为spark_rtabmap_slam_gazebo_rviz.launch，内容如下：
<?xml version="1.0"?>
<launch>
<!-- 设置launch文件的参数 -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="true"/>
<arg name="debug" default="false"/>
<arg name="model" default="$(find spark1_description)/urdf/spark1_description_rtabmap.urdf"/>
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<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>
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 运行robot_state_publisher节点，发布tf -->
<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>
<!-- 在gazebo中加载机器人模型-->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-unpause -urdf -model marobot1 -param robot_description -x 0.0 -y 0.0"/>
<!--创建新的终端，spark键盘控制 “wsad”分别代表“前后左右”-->
<node pkg="spark_teleop" type="keyboard_control.sh" name="kc_2d" />
</launch>【配置Rtabmap】
【Rtabmap配置文件】
编写spark_mapping_astrapro.launch文件启动Rtabmap
<launch>
<arg name="database_path" default="rtabmap.db"/>
<arg name="rgbd_odometry" default="false"/>
<arg name="rtabmapviz" default="false"/>
<arg name="localization" default="false"/>
<arg name="simulation" default="false"/>
<arg name="sw_registered" default="false"/>
<arg if="$(arg localization)" name="args" default=""/>
<arg unless="$(arg localization)" name="args" default="--delete_db_on_start"/>
<arg name="rgb_topic" default="/camera/rgb/image_raw"/>
<arg name="depth_topic" default="/camera/depth/image_raw"/>
<arg name="camera_info_topic" default="/camera/rgb/camera_info"/>
<arg name="wait_for_transform" default="0.2"/>
<!-- Mapping -->
<group ns="rtabmap">
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="map_negative_poses_ignored" type="bool" value="true"/>
<param name="queue_size" type="string" value="10"/>
<!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base -->
<param name="use_action_for_goal" type="bool" value="true"/>
<remap from="move_base" to="/move_base"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<!-- Fix odom covariance as in simulation the covariance in /odom topic is high (0.1 for linear and 0.05 for angular) -->
<param unless="$(arg rgbd_odometry)" name="odom_frame_id" value="odom"/>
<param unless="$(arg rgbd_odometry)" name="odom_tf_linear_variance" value="0.001"/>
<param unless="$(arg rgbd_odometry)" name="odom_tf_angular_variance" value="0.001"/>
<!-- output -->
<remap from="grid_map" to="/map"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Set to false to generate map correction between /map and /odom -->
<param name="Kp/MaxDepth" type="string" value="4.0"/>
<param name="Reg/Strategy" type="string" value="0"/> <!-- Loop closure transformation: 0=Visual, 1=ICP, 2=Visual+ICP -->
<param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
<param name="Vis/MinInliers" type="string" value="15"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.1"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
<param name="Rtabmap/TimeThr" type="string" value="0"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.30"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="GridGlobal/MinSize" type="string" value="20"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
<node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="Vis/InlierDistance" type="string" value="0.05"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
</node>
<!-- visualization with rtabmapviz -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="/scan"/>
</node>
</group>
</launch>主要修改rgb/image,depth/image,rgb/camera_info和/scan的相关话题，由于采用仿真环境，摄像机相关话题名称由URDF文件中确定，详细可查看上文深度摄像头部分
【点云转雷达信号】
编写pointcloud2scan.launch
<!--spark camera pointcloud to laserscan-->
<launch>
<arg name="point2laser" default="/depth/points"/>
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="$(arg point2laser)"/>
<remap from="scan" to="/scan" />
<rosparam>
target_frame: camera_link
transform_tolerance: 0.01
min_height: 0.0
max_height: 1.0
angle_min: -1.5708
angle_max: 1.5708
angle_increment: 0.0087
scan_time: 0.3333
range_min: 0.45
range_max: 4.0
use_inf: true
#concurrency_level affects number of pc queued for processing and the number of threadsused
# 0: Detect number of cores
# 1: Single threaded
# 2: inf : Parallelism level
concurrency_level: 1
</rosparam>
</node>
</launch>【整合启动文件】
将上面用到的节点编写到launch文件
<include file="$(find spark1_description)/launch/pointcloud2scan.launch">
<arg name="point2laser" value="$(arg point2laser)"/>
</include>
<!-- Move base -->
<include file="$(find spark_navigation)/launch/includes/move_base.launch.xml"/>
<!-- rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find spark_rtabmap)/rviz/spark_rtab_map_astrapro.rviz"/>
<!-- rtabmap -->
<include file="$(find spark_rtabmap)/launch/spark_mapping_astrapro.launch">
<arg name="localization" value="true"/>
</include>再将gazebo仿真和rtabmap结合一起，修改之前的spark_rtabmap_slam_gazebo_rviz.launch文件
<?xml version="1.0"?>
<launch>
<!-- 设置launch文件的参数 -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="true"/>
<arg name="debug" default="false"/>
<arg name="model" default="$(find spark1_description)/urdf/spark1_description_rtabmap.urdf"/>
<arg name="point2laser" default="/camera/depth_registered/points"/>
<!-- 运行gazebo仿真环境 -->
<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>
<!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<!-- 运行robot_state_publisher节点，发布tf -->
<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>
<!-- 在gazebo中加载机器人模型-->
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-unpause -urdf -model marobot1 -param robot_description -x 0.0 -y 0.0"/>
<include file="$(find spark1_description)/launch/pointcloud2scan.launch">
<arg name="point2laser" value="$(arg point2laser)"/>
</include>
<!-- Move base -->
<include file="$(find spark_navigation)/launch/includes/move_base.launch.xml"/>
<!-- rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find spark_rtabmap)/rviz/spark_rtab_map_astrapro.rviz"/>
<!-- rtabmap -->
<include file="$(find spark_rtabmap)/launch/spark_mapping_astrapro.launch">
<arg name="localization" value="true"/>
</include>
<!--创建新的终端，spark键盘控制 “wsad”分别代表“前后左右”-->
<node pkg="spark_teleop" type="keyboard_control.sh" name="kc_2d" />
<!--创建新的终端，确定是否保存地图-->
<node pkg="spark_teleop" type="cmd_save_map.sh" name="csm_2d" />
</launch>【运行Rtabmap仿真slam】
终端输入
roslaunch spark1_description spark_rtab_slam_gazebo_rviz.launch
启动后，添加简易围墙和障碍物
在rviz上查看
通过键盘控制完成建图
将Rtabmap cloud设为不显示
通过map_server保存地图
终端显示保存的位置