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

-- 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 = 1

MAP_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.66

return 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">
		<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>

【编写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">
		<origin
      xyz="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博客

地瓜机器人开发者社区

聚焦RDK与具身智能，汇集机器人开发教程、开源案例和技术交流

更多推荐

Minimax-h3_Singularity_v2_pruned完整指南：让中端显卡跑通AI视频生成的剪枝模型（新手必读）

**Minimax-h3_Singularity_v2_pruned** 为热门 AI 视频生成模型 MiniMax H3 提供了**剪枝 + 量化**后的精简版本（int8 / w6a8 / w4a8 三个文件），体积相比 bf16 原版大幅缩减，让显存只有 16GB～24GB 的中端显卡也能在 ComfyUI 中顺畅跑通文生视频、图生视频与参考视频生成（reference-to-video）。

地瓜机器人开发者社区

没有源码也能看穿软件？rea 用 MCP 让 AI 替你干逆向的活

你在 App 里看到某个功能特别顺手，心里琢磨"这到底是怎么实现的？"——没有源码，只有冷冰冰的编译产物。开源项目 rea（Reverse Engineer Anything，逆向一切）就是干这个的：它通过一个 MCP 服务器把逆向分析工具直接接进你的 AI 助手，让你用大白话提问，AI 就能在你本地把二进制、Electron 应用、.NET 程序甚至网站拆开看个明白，连证据和依据一并给你。[

地瓜机器人开发者社区

dsh-our-free-model 免费模型插件上手指南

想在 dsh 里写代码、聊天时直接用 DeepSeek V4.1 Flash 或 Kimi K3，又不想注册账号、不想填 API Key、不想算剩多少 token，dsh-our-free-model 就是直接的答案：把它作为插件挂进 dsh，输入框选择器里就会出现模型分组，完全免费、没有额度体系。当前版本 2.8.2，零依赖、无构建，安装就是一行 clone 加一行插件命令。## 先装起来