【踩坑记录】仿真环境使用小车进行Cartographer 3D Slam（深度摄像头）

【运行背景】

ROS1 20.04 noetic

安装cartographer请看：

【安装学习】安装Cartographer ROS（noetic）_Howe_xixi的博客-CSDN博客网上使用noetic安装cartographer的比较少，所以将安装步骤记录下来https://blog.csdn.net/weixin_44362628/article/details/122540297?spm=1001.2014.3001.5501gazebo仿真环境:如有以下问题请看：

【踩坑记录】Gazebo启动慢，画面卡“Preparing your world”_Howe_xixi的博客-CSDN博客启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501启动gazebo慢，卡顿，一直停留在Preparing your world的界面上https://blog.csdn.net/weixin_44362628/article/details/122512584?spm=1001.2014.3001.5501

【运行背景】

【Cartographer配置】

【修改lua文件】

【修改launch文件】

【重新编译】

【Gazebo仿真】

【小车URDF模型】

【编写launch文件】

【运行Cartographer仿真】

【问题总结】

【如何添加传感器】

【解决点云显示错误的问题】

【感谢】

【Cartographer配置】

【修改lua文件】

cartographer_ros配置文件.lua文件路径位于cartographer_ros/configuration_files中，选择其中的backpack_3d.lua文件复制修改，取新名字为spark_pointcloud.lua，内容如下：

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.include "map_builder.lua"
include "trajectory_builder.lua"options = {map_builder = MAP_BUILDER,trajectory_builder = TRAJECTORY_BUILDER,map_frame = "map", tracking_frame = "base_footprint",published_frame = "odom",odom_frame = "odom",provide_odom_frame = false,publish_frame_projected_to_2d = false,use_pose_extrapolator = false,use_odometry = true,use_nav_sat = false,use_landmarks = false,num_laser_scans = 0,num_multi_echo_laser_scans = 0,num_subdivisions_per_laser_scan = 5,num_point_clouds = 1,lookup_transform_timeout_sec = 0.2,submap_publish_period_sec = 0.3,pose_publish_period_sec = 5e-3,trajectory_publish_period_sec = 30e-3,rangefinder_sampling_ratio = 1.,odometry_sampling_ratio = 1.,fixed_frame_pose_sampling_ratio = 1.,imu_sampling_ratio = 1.,landmarks_sampling_ratio = 1.,
}TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66return options

主要修改以下内容：

a) racking_frame = "base_footprint",仿真时用base_footprint,实体时用imu_link;

b) TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1，设置太大会导致数据处理错误

*其他参数值含义可参考官网，链接见以下链接

Cartographer ROS Integration

【修改launch文件】

cartographer_ros的launch文件backpack_3d.launch,路径位于/cartographer_ros/launch中，选择并进行复制修改，取新名字spark_cartographer_3d.launch，内容如下：

<?xml version="1.0"?>
<launch><node name="cartographer_node" pkg="cartographer_ros"type="cartographer_node" args="-configuration_directory $(find cartographer_ros)/configuration_files-configuration_basename spark_pointcloud.lua"output="screen"><remap from="/points2" to="/camera/depth/points" /></node><node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"type="cartographer_occupancy_grid_node" args="-resolution 0.05" /><node name="rviz" pkg="rviz" type="rviz" required="true"args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" /></launch>

主要启动以下内容：

a) 启动cartographer_node节点，配置文件指向刚刚设置的.lua文件，同时映射points2对应的话题

b) 启动cartographer_occupancy_grid_node地图格式转换节点，由于cartographer_node建图节点提供的地图是submapList格式的，需要转换成GridMap格式才能在ROS中显示和使用。这里面有两个可配参数，resolution用来设置GridMap地图的分辨率，publish_period_sec用来设置GridMap地图发布的频率。

c) 启动Rviz，启动成功后，再根据需求保存配置即可

【重新编译】

通过Lua脚本配置参数的方法，每次修改参数后需要重新编译，否则参数无法生效

catkin_make_isolated --install --use-ninja

【Gazebo仿真】

均在spark-slam-gazebo工作空间下

【小车URDF模型】

Cartographer需要imu和pointcloud的数据，所以添加这两部分的传感器

a）添加深度摄像头

  <link name="virtual_camera_link" /><joint name="virtual_camera_joint"type="fixed"><originxyz="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><jointname="imu_joint"type="fixed"><originxyz="-0.0 0.0 0.0"rpy="0 0 0" /><parentlink="base_footprint" /><childlink="imu_link" /><axisxyz="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>

【编写launch文件】

取名为spark_cartographer_slam.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_cartographer.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>

该launch文件包含以下五个部分：

运行Gazebo仿真环境
运行joint_state_publisher节点，发布机器人的关节状态
运行robot_state_publisher节点，发布tf
在gazebo中加载机器人模型
运行键盘控制节点

【运行Cartographer仿真】

a ) 启动仿真环境

roslaunch spark1_description spark_cartographer_slam.launch

添加上自己搭的简易模型，添加障碍物

b ) 启动Cartographer

roslaunch cartographer_ros spark_cartographer_3d.launch

机器人出现漂移现象，导致建图效果不好，后续会进行优化

【问题总结】

【如何添加传感器】

在gazebo中可以通过插入一些插件，来仿真机器人的传感器、执行器的特性，这些插件通过<gazebo>元素中的<plugin>标签描述，使用下边的格式加入到URDF文件中（详细请看上文添加camera和imu）：

<gazebo reference="your_link_name">
<gazebo><plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">... plugin parameters ...</plugin>
</gazebo>

<gazebo reference>指的是插件附着的位置，比如机器人的某个link，gazebo默认支持不少常用的设备，而且也放置在ros安装目录下（也有在gazebo_plugins功能包中）

同时添加一个虚拟的link和joint将插件固定在机器人上

  <link name="virtual_camera_link" /><joint name="virtual_camera_joint"type="fixed"><originxyz="0 0 0.03"rpy="0 0 0" /><parent link="camera_Link" /><child link="virtual_camera_link" /></joint>

【解决点云显示错误的问题】

曾经出现过点云显示位置错误，调用camera查看图形没有问题，在rviz上显示错误，如下图：

解决办法：

a）添加一个虚拟的关节，关联到摄像机的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>

b）将深度摄像头插件中的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>

再次运行仿真环境，点云就会正确显示

【感谢】

ROS探索总结（二十四）——使用gazebo中的插件 - 古月居

Gazebo urdf kinetic RGBD camera 相机视角和点云视角不一致_Lucky Vivi的博客-CSDN博客