沉默杀手HomeArchivesTagsCategories
cartographer_offline_node
|激光SLAMCartographer源码解读|
Word count: 833|Reading time: 3 min

cartographer_offline_node对应offline_node_main.cc，它和cartographer_grpc_offline_node对应的offline_node_grpc_main.cc极为相似

在线和离线的建图效果无区别。离线节点直接从bag读取传感器数据，使用了CPU能实现的最大速度处理bag，而且不会丢失消息，不过无法设置速度和看不清建图过程的细节。 如果想控制处理速度，就使用在线节点和rosbag play -r 2，但是如果设置速度太快，可能会丢失消息，这是很常见的。

简单看了源码，离线节点之所以运行快，是因为使用了ROS多线程ros::AsyncSpinner，而在线节点还是普通的ROS程序

运行示例

1
2
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag

此时的CPU占用果然要爆了，所以说只适合运行数据集，不适合需要优化cartographer的场合
CPU占用.png

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/offline_node.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "ros/ros.h"

int main(int argc, char** argv)
{
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  ::ros::init(argc, argv, "cartographer_offline_node");
  ::ros::start();
  cartographer_ros::ScopedRosLogSink ros_log_sink;

  const cartographer_ros::MapBuilderFactory   map_builder_factory =
      [](const ::cartographer::mapping::proto::MapBuilderOptions& 
             map_builder_options) {
        return absl::make_unique< ::cartographer::mapping::MapBuilder>(
            const_cast<::cartographer::mapping::proto::MapBuilderOptions&>(map_builder_options));
      };
  cartographer_ros::RunOfflineNode(map_builder_factory);
  ::ros::shutdown();
}

参数

RunOfflineNode函数转到了offline_node.cc，这个文件主要就是这个函数。这个文件开头是一堆宏定义，根据字符串解释每个可用的参数

  • configuration_directory
  • configuration_basenames

  • load_state_filename

  • bag_filenames: Comma-separated list of bags to process. One bag per trajectory. Any combination of simultaneous and sequential bags is supported.

  • urdf_filenames: Comma-separated list of one or more URDF files that contain. static links for the sensor configurations

  • use_bag_transforms: 默认true，Whether to read, use and republish transforms from bags

  • keep_running: 默认false，最好改为true。Keep running the offline node after all messages from the bag have been processed

  • load_frozen_state: 默认true，Load the saved state as frozen (non-optimized) trajectories

  • skip_seconds: 默认0，Optional amount of seconds to skip from the beginning (i.e. when the earliest bag starts)

一个配置了两个机器人的的launch的关键部分是这样的:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
<node name="cartographer_offline_node" pkg="cartographer_ros"
  required="$(arg no_rviz)"
  type="cartographer_offline_node" args="
      -configuration_directory $(arg abspath)
      -configuration_basenames alpha-cartographer.lua,delta-cartographer.lua
      -urdf_filenames $(arg abspath)/pioneer-alpha.urdf,$(arg abspath)/pioneer-delta.urdf
      -bag_filenames $(arg abspath)/alpha.bag,$(arg abspath)/delta.bag
      -keep_running=true -use_bag_transforms=false"
  output="screen">
  <remap from="bag_1_scan" to="alpha/scan" />
  <remap from="bag_1_odom" to="alpha/pose" />
  <remap from="bag_2_scan" to="delta/scan" />
  <remap from="bag_2_odom" to="delta/pose" />
</node>

话题和service

Publications:

  • cartographer_offline_node/bagfile_progress [cartographer_ros_msgs/BagfileProgress]
  • clock [rosgraph_msgs/Clock]
  • constraint_list [visualization_msgs/MarkerArray]
  • landmark_poses_list [visualization_msgs/MarkerArray]
  • rosout [rosgraph_msgs/Log]
  • scan_matched_points2 [sensor_msgs/PointCloud2]
  • submap_list [cartographer_ros_msgs/SubmapList]
  • tf [tf2_msgs/TFMessage]
  • tf_static [tf2_msgs/TFMessage]
  • tracked_pose [geometry_msgs/PoseStamped]
  • trajectory_node_list [visualization_msgs/MarkerArray]

Subscriptions:

  • clock [rosgraph_msgs/Clock]

Services:

  • finish_trajectory
  • get_trajectory_states
  • read_metrics
  • start_trajectory
  • submap_query
  • trajectory_query
  • write_state

离线节点是直接在程序里读取消息，所以它没有像rosbag play那样把bag里的话题发布出来，我运行bag时，因为还需要运行一个节点对bag里的话题进行类型转换，而话题又没发布出来，所以离线节点实际无法使用了。

源码的重要部分

1
2
3
4
5
6
7
8
9
constexpr char kClockTopic[] = "clock";
constexpr char kTfStaticTopic[] = "/tf_static";
constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1;
// We publish tf messages one second earlier than other messages. Under
// the assumption of higher frequency tf this should ensure that tf can
// always interpolate.
const ::ros::Duration kDelay = ::ros::Duration(1.0);