沉默杀手HomeArchivesTagsCategories
(二) 综述, 话题和服务
|激光SLAMCartographer原理和配置|
Word count: 1.4k|Reading time: 6 min

Cartographer是基于图优化的SLAM，采用误差累计少，计算成本低的Scan to Map匹配方式，而不是造成误差快速累积和计算成本高的scan to scan匹配。整个算法分为局部SLAM和全局SLAM，两部分都对雷达观测的位姿进行了优化。

虽然cartographer适用于2D和3D两种情况，但3D情况下，cartographer用的不如LOAM多

订阅的话题

节点cartographer_node订阅的话题是：scan, tf, tf_static。 cartographer_node的launch需要增加remap，因为它接受的话题名称和我们现有的话题可能不一致：

1
2
3
<remap from="scan"  to="/scan" />
<remap from="odom"  to="/xqserial_server/Odom" />
<remap from="imu"   to="/xqserial_server/IMU" />

所有相关的话题名称都定义在node_constants.h

cartographer_node 发布的话题

  • scan_matched_points2, 类型sensor_msgs/PointCloud2, scan-to-submap匹配的2D点云数据，这个云可以根据配置进行滤波和投影。是scan话题的大部分，不会包括行走的人的轮廓

  • submap_list, 类型cartographer_ros_msgs/SubmapList，所有轨迹的所有submaps的列表，成员：

    1
    2
    3
    4
    5
    6
    7
    8
    std_msgs/Header header
    cartographer_ros_msgs/SubmapEntry[] submap

    # 其中submap的类型如下
    int32 trajectory_id
    int32 submap_index
    int32 submap_version
    geometry_msgs/Pose pose

    话题包括一个子图的ID list(包括轨迹ID和子图index)，以及子图的全局位姿。

发布话题的源码流程是 Node::PublishSubmapList —— MapBuilderBridge::GetSubmapList() —— PoseGraph2D::GetAllSubmapPoses() —— PoseGraph2D::GetSubmapDataUnderLock —— PoseGraph2D::ComputeLocalToGlobalTransform

  • trajectory_node_list，类型为visualization_msgs/MarkerArray，即轨迹路径node列表，代码是
1
2
3
4
5
6
7
8
9
10
void Node::PublishTrajectoryNodeList(
    const ::ros::WallTimerEvent& unused_timer_event)
{
  if (trajectory_node_list_publisher_.getNumSubscribers() > 0)
  {
    carto::common::MutexLocker lock(&mutex_);
    trajectory_node_list_publisher_.publish(
        map_builder_bridge_.GetTrajectoryNodeList()  );
  }
}

  • constraint_list: 类型为visualization_msgs/MarkerArray，约束列表

  • landmark_poses_list: 类型为visualization_msgs/MarkerArray，路标点位姿列表

节点 cartographer_occupancy_grid_node 发布话题 map(栅格地图)， 订阅submap_list [cartographer_ros_msgs/SubmapList]。 cartographer_occupancy_grid_node 节点将子图转换为ROS的地图格式

服务

  • submap_query (cartographer_ros_msgs/SubmapQuery)： 查询submap的服务，输入的是trajectory_id 和 submap_index
    call submap_query

start_trajectory

类型cartographer_ros_msgs/StartTrajectory，通过将其传感器主题和轨迹选项指定为二进制编码的原型来启动另一个轨迹。返回指定的轨迹ID。

StartTrajectory.srv:

1
2
3
4
5
6
7
8
string   configuration_directory
string   configuration_basename
bool   use_initial_pose
geometry_msgs/Pose   initial_pose
int32   relative_to_trajectory_id
---
cartographer_ros_msgs/StatusResponse   status
int32   trajectory_id

命令稍微长了点:

1
2
3
4
5
6
7
rosservice call /start_trajectory "configuration_directory: '/home/user/carto_ws/src/cartographer_ros/cartographer_ros/configuration_files'
configuration_basename: 'localization.lua'
use_initial_pose: true
initial_pose:
  position: {x: 5.211, y: 5.617, z: 0.0}
  orientation: {x: 0.0, y: 0.0, z: 0.689, w: 0.725}
relative_to_trajectory_id: 0"

出错的地方可能会在relative_to_trajectory_id：

1
2
3
4
5
6
7
status:
  code: 5
  message: "Trajectory 6 doesn't exist."

status:
  code: 3
  message: "Trajectory 5 is in 'DELETED' state."

加载 frozen state 意味着不插入no pose graph constraints. 一条frozen trajectory和它的子图不受 optimization影响

finish_trajectory

类型cartographer_ros_msgs/FinishTrajectory: Finish 一个给定ID的轨迹，方式是运行最终的图优化。

1
2
3
int32 trajectory_id
---
cartographer_ros_msgs/StatusResponse status

命令rosservice call finish_trajectory "trajectory_id: 5"，得到这样的结果:

1
2
3
4
status:
  code: 3
  message: "Topics are already used by another trajectory."
trajectory_id: 0

trajectory_query

  • get_trajectory_states (cartographer_ros_msgs/GetTrajectoryStates): 返回所有轨迹的ID和状态， 可以用于从单独的节点观察Cartographer的状态。
    1
    2
    3
    4

    ---
    cartographer_ros_msgs/StatusResponse   status
    cartographer_ros_msgs/TrajectoryStates  trajectory_states

命令rosservice call /get_trajectory_states "{}"，比如得到这样的结果：

1
2
3
4
5
6
7
8
9
10
11
12
status:
  code: 0
  message: ''
trajectory_states:
  header:
    seq: 0
    stamp:
      secs: 1612232712
      nsecs:  41338148
    frame_id: ''
  trajectory_id: [0, 1, 2, 3, 4, 5]
  trajectory_state: [2, 2, 2, 2, 2, 0]

trajectory_state的枚举值也就是 轨迹的四种状态对应: ACTIVE, FINISHED, FROZEN, DELETED

  • write_state (cartographer_ros_msgs/WriteState): 将当前内部状态写入磁盘到文件名。用于保存pbstream文件，此文件可用作assets_writer_main的输入，以生成概率网格，X-Rays或PLY文件等资源

read_metrics

类型(cartographer_ros_msgs/ReadMetrics)， 返回Cartographer的所有内部指标的最新值。运行时度量标准的集合(collection of runtime metrics)是可选的，必须使用节点中的-collect_metrics命令行标志激活:

1
2
3
4
5
6
7
8
9
<node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename map.lua
          -collect_metrics true"
      output="screen">
  <remap from="scan" to="/scan_rectified" />
  <remap from="odom" to="/odom" />
</node>

如果没有加-collect_metrics标志，rosservice call /read_metrics "{}"的结果:

1
2
3
4
5
6
7
status:
  code: 14
  message: "Collection of runtime metrics is not activated."
metric_families: []
timestamp:
  secs: 1612333336
  nsecs: 359502516

需要的tf变换

必须提供从所有传入传感器数据帧到配置的tracking_frame和published_frame的转换。通常，这些是由robot_state_publisher或static_transform_publisher定期发布的。

发布的tf变换

提供map_frame和 published_frame之间的转换。如果在Lua中启用了provide_odom_frame，则将提供配置的odom_frame和published_frame之间的连续（不受循环闭包影响）转换。

其他节点

cartographer_ros的所有节点.png

  • cartographer_rosbag_validate

可以检测bag中的错误，使用：

1
rosrun cartographer_ros cartographer_rosbag_validate -bag_filename <bag filename>

比如会有这样的结果：
1
2
3
4
5
6
7
8
9
10
11
12
13
E0722 22:11:01.138546  3442 rosbag_validate_main.cc:389] IMU data (frame_id: "imu") has a large gap, largest is 0.064731 s, recommended is [0.0005, 0.005]s with no jitter.
I0722 22:11:01.139314  3442 rosbag_validate_main.cc:398] Time delta histogram for consecutive messages on topic "/IMU_data" (frame_id: "imu"):
Count: 27679  Min: 0.000144  Max: 0.064731  Mean: 0.005027
[0.000144, 0.006602)    					    Count: 27445 (99.154594%)    Total: 27445 (99.154594%)
[0.006602, 0.013061)                            Count: 181 (0.653925%)  Total: 27626 (99.808517%)
[0.013061, 0.019520)                            Count: 42 (0.151740%)   Total: 27668 (99.960258%)
[0.019520, 0.025979)                            Count: 7 (0.025290%)    Total: 27675 (99.985550%)
[0.025979, 0.032437)                            Count: 3 (0.010839%)    Total: 27678 (99.996384%)
[0.032437, 0.038896)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.038896, 0.045355)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.045355, 0.051814)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.051814, 0.058272)                            Count: 0 (0.000000%)    Total: 27678 (99.996384%)
[0.058272, 0.064731]                            Count: 1 (0.003613%)    Total: 27679 (100.000000%)

需要的时间范围是[0.0005, 0.005]，但是看给出的范围列表，只有第一个还算在这个范围里，剩下约1%不合要求。

  • cartographer_pbstream_map_publisher

一个简单的节点，它从序列化的Cartographer状态（pbstream格式）创建静态占用网格。如果实时更新不重要，它是占用网格节点的有效替代方案。 发布话题：map (nav_msgs/OccupancyGrid)，只发布一次