cartographer_node的外墙——node对象

在分析源文件node_main.cc的时候，我们猜测其在Run函数中构建的局部对象node实际控制着整个系统的业务逻辑。 本文中，我们来研读与这个对象相关的源文件node.cc和node.h，了解cartographer_node是如何将ROS系统的消息和服务与Cartographer联系起来的。

1. 构造函数

下面是node对象类型cartographer_ros::Node构造函数的片段，它有三个输入参数。其中，node_options是node对象的各种配置参数，在node_main.cc中由函数Run中从配置文件中获取。map_builder是Cartographer用于建图的对象，而tf_buffer是ROS系统中常用的坐标变换库tf2的缓存对象。代码片段中的第3行的部分是C++标准的构造时对成员变量进行初始化的方法。

        Node::Node(const NodeOptions& node_options,
            std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, tf2_ros::Buffer* const tf_buffer)
            : node_options_(node_options), map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {

在构造函数一开始，先对互斥量mutex_加锁，防止因为多线程等并行运算的方式产生异常的行为。这里定义的是局部变量lock，会在其构造函数中对mutex_加锁， 当构造函数运行结束之后会销毁该变量，在其析构函数中释放mutex_。

            carto::common::MutexLocker lock(&mutex_);

接着通过ROS的节点句柄node_handle_注册发布了一堆主题。这里以前缀"k"开始的变量都是在node_constants.h中定义的常数，它们都是默认的发布消息的名称和消息队列大小。

            submap_list_publisher_ = node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(kSubmapListTopic, kLatestOnlyPublisherQueueSize);
            trajectory_node_list_publisher_ = node_handle_.advertise<::visualization_msgs::MarkerArray>(kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
            landmark_poses_list_publisher_ = node_handle_.advertise<::visualization_msgs::MarkerArray>(kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
            constraint_list_publisher_ = node_handle_.advertise<::visualization_msgs::MarkerArray>(kConstraintListTopic, kLatestOnlyPublisherQueueSize);
            scan_matched_point_cloud_publisher_ = node_handle_.advertise<sensor_msgs::PointCloud2>(kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);

这五个主题的默认名称和说明如下表所示：

然后注册服务，并保存在容器service_servers_中。同样的前缀"k"开始的变量都是在node_constants.h中定义的常数，它们是默认的服务名称。 在注册服务的时候还需要提供一个响应该服务的回调函数，我们暂时不分析服务。

            service_servers_.push_back(node_handle_.advertiseService(kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
            service_servers_.push_back(node_handle_.advertiseService(kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
            service_servers_.push_back(node_handle_.advertiseService(kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
            service_servers_.push_back(node_handle_.advertiseService(kWriteStateServiceName, &Node::HandleWriteState, this));

最后Node的构造函数又创建了一堆计时器保存在容器wall_timers_中，这些计时器的作用是通过超时回调函数定时的发布对应主题的消息。

            wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.submap_publish_period_sec), &Node::PublishSubmapList, this));
            wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.pose_publish_period_sec), &Node::PublishTrajectoryStates, this));
            wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.trajectory_publish_period_sec), &Node::PublishTrajectoryNodeList, this));
            wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.trajectory_publish_period_sec), &Node::PublishLandmarkPosesList, this));
            wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(kConstraintPublishPeriodSec), &Node::PublishConstraintList, this));
        }

2. 开始轨迹跟踪

在node_main.cc的函数Run中，通过调用node对象的成员函数StartTrajectoryWithDefaultTopics来开始轨迹跟踪。 除此之外，还可以调用服务"start_trajectory"来开启。两者的不同是，前者使用系统默认的订阅主题，后者在调用服务的时候还需要提供轨迹配置和订阅主题。下面是StartTrajectoryWithDefaultTopics的函数实现。 内容很简单，就三条语句。先加锁，在检查输入的配置是否合法，最后调用函数AddTrajectory开始轨迹跟踪。通过服务的形式开启轨迹跟踪，最终也是调用这个AddTrajectory。

        void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
            carto::common::MutexLocker lock(&mutex_);
            CHECK(ValidateTrajectoryOptions(options));
            AddTrajectory(options, DefaultSensorTopics());
        }

下面是AddTrajectory的代码片段，首先通过函数ComputeExpectedSensorIds根据配置选项options获取SendorId。所谓的SensorId是定义在trajectory_builder_interface.h中的一个结构体，它一共有两个字段，type通过枚举描述了传感器的类型，id是一个字符串记录了传感器所对应的ROS主题名称。

        int Node::AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) {
            const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
                expected_sensor_ids = ComputeExpectedSensorIds(options, topics);

接着通过接口map_builder_bridge_向Cartographer添加一条新的轨迹并获取轨迹的索引。然后以该索引为键值，通过函数AddExtrapolator和AddSensorSamplers添加用于位姿插值(PoseExtrapolator)和传感器采样(TrajectorySensorSamplers)的对象。这里的位姿插值和传感器采样是根据数据类型的名字猜的，具体完成什么功能以后用到的时候在具体分析。

            const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
            AddExtrapolator(trajectory_id, options);
            AddSensorSamplers(trajectory_id, options);

然后调用成员函数LaunchSubscribers完成传感器消息的订阅。这是一个关键的步骤，只有订阅了传感器消息，才能够在传感器数据的作用下驱动系统运转，进而完成位姿估计和建图的任务。

            LaunchSubscribers(options, topics, trajectory_id);

最后记录当前正在更新的轨迹，以及订阅的主题，并返回新建的轨迹索引。

            is_active_trajectory_[trajectory_id] = true;
            for (const auto& sensor_id : expected_sensor_ids)
                subscribed_topics_.insert(sensor_id.id);
            return trajectory_id;
        }

3. 订阅传感器主题

按照ROS的套路，应用程序一般都可以说是数据驱动的。也就是说，节点会订阅来自不同数据源的主题，每当接收到感兴趣的消息就会通过一个回调函数完成对应的处理。 在node对象中，通过成员函数LaunchSubscribers根据配置订阅需要的主题。

下面是这个函数的代码片段。在该函数中通过定义在node.cc中的模板函数SubscribeWithHandler调用ROS的节点句柄node_handle_来订阅传感器的主题。容器对象subscribers_中保存的是构建的订阅器，其中元素的数据类型是Node类型中定义的私有结构体Subscriber，有两个字段分别记录了订阅器和传感器主题。第2行中的for循环写法是C++11标准中新增的"for range"形式，函数ComputeRepeatedTopicNames是用来处理有多个相同类型的传感器的。

比如这里的单线激光laser_scan，通过在配置文件中的num_laser_scans字段指定单线激光的数量。假如输入参数topics.laser_scan_topic中对应的主题名称是"scan"，那么如果只有一个单线激光，就是用scan作为订阅主题名称，如果有多个单线激光则在主题名称之后在添加添加数字予以区别，即scan_1, scan_2, ...

        void Node::LaunchSubscribers(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics, const int trajectory_id) {
            for (const std::string& topic : ComputeRepeatedTopicNames(topics.laser_scan_topic, options.num_laser_scans)) {
                subscribers_[trajectory_id].push_back({
                    SubscribeWithHandler(&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,this),
                    topic
                });
            }

类似的多线激光扫描数据(multi_echo_laser_scan)和点云数据(point_clouds)都以相同的套路完成主题订阅，这里不再赘述，详细参考源文件。下面是订阅IMU传感器话题的代码片段，如果是三维建图就必须使用IMU，二维建图可以通过配置文件中的use_imu_data字段设置。

            if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
               (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options().use_imu_data())) {
                std::string topic = topics.imu_topic;
                subscribers_[trajectory_id].push_back({
                    SubscribeWithHandler(&Node::HandleImuMessage,trajectory_id, topic, &node_handle_, this),
                    topic
                });
            }

此外该函数中还有关于里程计(Odometry)、导航卫星(NavSatFix)以及路标(Landmark)的数据源订阅，这里省略了，详细参考源文件。

4. 订阅回调函数

        $ rosnode info /cartographer_node 
        ------------------------------------------------------------------
        Subscriptions: 
         * /clock [rosgraph_msgs/Clock]
         * /horizontal_laser_2d [sensor_msgs/MultiEchoLaserScan]
         * /imu [sensor_msgs/Imu]
         * /tf [tf2_msgs/TFMessage]
         * /tf_static [tf2_msgs/TFMessage]

在运行官方2D建图的demo的时候， 我们可以通过rosnode工具查看cartographer_node的详细信息，如右侧代码片段所示，我们只截取了它的订阅消息部分的输出日志。

可以看到它只订阅了5个主题，其中"/tf"和"/tf_static"是与系统坐标变换相关的消息，由ROS的tf2库负责。"/clock"是由ROS的录包回放系统rosbag发布的主题，主要是提供一个仿真时间。 实际与传感器相关的主题只有"/horizontal_laser_2d"和"/imu"两个，也就是说我们的demo里面只是用了一个激光传感器和一个IMU。那么接下来，我们就分析这两个传感器数据的消息回调函数。

下面的代码片段是激光传感器的消息回调函数，在该函数的一开始先加锁，然后通过采样器对传感器的数据进行降采样之后，通过map_builder_bridge_将传感器数据喂给Cartographer。这里需要啰嗦两句的是这个采样器， 它的数据类型是定义在node.h中的结构体TrajectorySensorSamplers， 在函数AddTrajectory中通过调用AddSensorSamplers完成初始化操作。 其本质是对cartographer中的采样器(fixed_ratio_sampler.h, fixed_ratio_sampler.cc)的封装， 用一个计数器来按照一个指定的频率对原始的数据进行降采样，采样频率可以通过轨迹参数文件来配置。我们会在后序的文章中对这个采样器进行详细的分析。

        void Node::HandleMultiEchoLaserScanMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
            carto::common::MutexLocker lock(&mutex_);
            if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse())
                return;
            map_builder_bridge_.sensor_bridge(trajectory_id)->HandleMultiEchoLaserScanMessage(sensor_id, msg);
        }

下面是处理IMU的消息回调函数，它的套路和激光传感器的一样，都是降采样之后喂给Cartographer。所不同的是，除了要通过map_builder_bridge_喂给建图器之外，还需要将IMU数据喂给位姿估计器。 这个位姿估计器的数据类型是定义在cartographer的PoseExtrapolator， 同样是在函数AddTrajectory中通过调用AddExtrapolator完成初始化操作。 我们将在以后分析Cartographer内核的时候详细研究它。

        void Node::HandleImuMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) {
            carto::common::MutexLocker lock(&mutex_);
            if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse())
                return;
            auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
            auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
            if (imu_data_ptr != nullptr)
                extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
            sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
        }

5. 服务响应函数

        $ rosnode info /cartographer_node 
        ------------------------------------------------------------------
        Services: 
         * /cartographer_node/get_loggers
         * /cartographer_node/set_logger_level
         * /finish_trajectory
         * /start_trajectory
         * /submap_query
         * /write_state       

我们仍然使用rosnode工具来查看cartographer_node注册的服务，如右侧的代码片段所示。

输出的日志中，前两个服务是ROS系统的默认服务，用于查询当前的日志等级和设定日志等级。"/start_trajectory"和"/finish_trajectory"，分别用于触发开始路径跟踪和结束路径跟踪。 服务"/submap_query"用于查询子图，而"/write_state"则用于将当前的系统状态保存到指定文件中。

下面是"/start_trajectory"的服务响应函数的代码片段，在函数主体中，先通过两个条件语句来检查输入参数。如果输入参数合法则通过函数AddTrajectory按照服务请求的订阅主题开启一个路径跟踪， 并填充返回消息的相关字段，否则报错返回。

        bool Node::HandleStartTrajectory(::cartographer_ros_msgs::StartTrajectory::Request& request, ::cartographer_ros_msgs::StartTrajectory::Response& response) {
            carto::common::MutexLocker lock(&mutex_);
            TrajectoryOptions options;
            if (!FromRosMessage(request.options, &options) || !ValidateTrajectoryOptions(options)) {
                // 报错
            } else if (!ValidateTopicNames(request.topics, options)) {
                // 报错
            } else {
                response.trajectory_id = AddTrajectory(options, request.topics);
                response.status.code = cartographer_ros_msgs::StatusCode::OK;
                response.status.message = "Success.";
            }
            return true;
        }

下面是"/finish_trajectory"的服务响应函数的代码片段，没什么好说的就是调用函数FinishTrajectoryUnderLock停止路径跟踪。关于停止路径跟踪的过程中都做了些什么事情， 我们将在最后分析退出机制的时候介绍。

        bool Node::HandleFinishTrajectory(::cartographer_ros_msgs::FinishTrajectory::Request& request, ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
            carto::common::MutexLocker lock(&mutex_);
            response.status = FinishTrajectoryUnderLock(request.trajectory_id);
            return true;
        }

类似的"/submap_query"和"/write_state"通过map_builder_bridge_对象向Cartographer请求子图和状态，并将结果填充到响应消息中或者序列化到指定的文件中，这里不再列出代码了。

6. 完

本文中我们先分析了node对象的构造函数以及传感器消息订阅相关的函数，然后查看了消息回调函数与服务响应函数。 可以看到ROS系统与Cartographer内核之间的信息交换基本上都是通过对象map_builder_bridge_来完成的。