carto笔记--- 传感器数据走向

node.cc 的数据走向有两个

成员变量

  const NodeOptions node_options_;
  tf2_ros::TransformBroadcaster tf_broadcaster_;
  absl::Mutex mutex_;
  std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_;

  MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

  // These are keyed with 'trajectory_id'.
  std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
  std::map<int, ::ros::Time> last_published_tf_stamps_;
  std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
  std::unordered_map<int, std::vector<Subscriber>> subscribers_;
  std::unordered_set<std::string> subscribed_topics_;
  std::unordered_set<int> trajectories_scheduled_for_finish_;

数据走向有两个
一个是 cartographer::mapping::PoseExtrapolator , 一个是 map_builder_bridge_ 的 sensor_bridge()

void Node::HandleImuMessage(const int trajectory_id,
                            const std::string& sensor_id,
                            const sensor_msgs::Imu::ConstPtr& msg) {
    
    
  absl::MutexLock 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);
  // extrapolators_使用里程计数据进行位姿预测
  if (imu_data_ptr != nullptr) {
    
    
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

node.cc 调用 MapBuilderBridge的sensor_bridge,
MapBuilderBridge的sensor_bridge将数据传入trajectory_builder_interface
collated_trajectory_builder继承trajectory_builder_interface,
collator 继承 collated_trajectory_builder

MapBuilderBridge

成员变量

  const NodeOptions node_options_;
  std::unordered_map<int,
                     std::shared_ptr<const 
  tf2_ros::Buffer* const tf_buffer_;
  std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;

  // These are keyed with 'trajectory_id'.
  std::unordered_map<int, TrajectoryOptions> trajectory_options_;
  std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
  
  std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
  LocalTrajectoryData::LocalSlamData>> local_slam_data_ GUARDED_BY(mutex_);
  std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;

MapBuilderBridge 构造函数

/**
 * @brief 根据传入的node_options，MapBuilder，以及tf_buffer 完成三个本地变量的初始化
 * 
 * @param[in] node_options 参数配置
 * @param[in] map_builder 在node_main.cc中传入的MapBuilder
 * @param[in] tf_buffer tf_buffer
 */
MapBuilderBridge::MapBuilderBridge(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer)
    : node_options_(node_options),
      map_builder_(std::move(map_builder)),
      tf_buffer_(tf_buffer) {
    
    }

sensor_bridges_ 的初始化

// 开始一条新轨迹
int MapBuilderBridge::AddTrajectory(

  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  // 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id));

map_builder_->GetTrajectoryBuilder(trajectory_id) 指的是 CollatedTrajectoryBuilder

    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
  }

CollatedTrajectoryBuilder 是将前端local_trajectory_builder 与 后端 PoseGraph2D 结合在一起的 TrajectoryBuilder

SensorBridge

成员变量

  const int num_subdivisions_per_laser_scan_;
  std::map<std::string, cartographer::common::Time>
      sensor_to_previous_subdivision_time_;
  const TfBridge tf_bridge_;
  ::cartographer::mapping::TrajectoryBuilderInterface* const
      trajectory_builder_;
  absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_;

构造函数

// 构造函数，并且初始化TfBridge
SensorBridge::SensorBridge(
    const int num_subdivisions_per_laser_scan,
    const std::string& tracking_frame,
    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
      trajectory_builder_(trajectory_builder) {
    
    }

数据走向 SensorBridge -> CollatedTrajectoryBuilder::AddSensorData()

// 调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
                                    const sensor_msgs::Imu::ConstPtr& msg) {
    
    
  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
  if (imu_data != nullptr) {
    
    
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::ImuData{
    
    imu_data->time, imu_data->linear_acceleration,
                               imu_data->angular_velocity});
  }
}

MapBuilder

成员变量

  const proto::MapBuilderOptions options_;
  common::ThreadPool thread_pool_;
  std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
      all_trajectory_builder_options_;
      
  std::unique_ptr<PoseGraph> pose_graph_;
  std::unique_ptr<sensor::CollatorInterface> sensor_collator_;
  std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
      trajectory_builders_;

一些类的初始化

// pose_graph_
pose_graph_ = absl::make_unique<PoseGraph2D>(
    options_.pose_graph_options(),
    absl::make_unique<optimization::OptimizationProblem2D>(
        options_.pose_graph_options().optimization_problem_options()),
    &thread_pool_);

// sensor_collator_
sensor_collator_ = absl::make_unique<sensor::Collator>();

// local_trajectory_builder(前端)的初始化
local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
      trajectory_options.trajectory_builder_2d_options(),
      SelectRangeSensorIds(expected_sensor_ids));

// tag: CollatedTrajectoryBuilder初始化
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
    trajectory_options, sensor_collator_.get(), trajectory_id,
    expected_sensor_ids,
    // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
    CreateGlobalTrajectoryBuilder2D(
        std::move(local_trajectory_builder), trajectory_id,
        static_cast<PoseGraph2D*>(pose_graph_.get()),
        local_slam_result_callback, pose_graph_odometry_motion_filter)));

数据走向

CollatedTrajectoryBuilder

sensor_bridges_ 的 AddSensorData 调用的是 CollatedTrajectoryBuilder 中的函数.

成员变量

  sensor::CollatorInterface* const sensor_collator_;
  const bool collate_landmarks_;
  const bool collate_fixed_frame_;
  const int trajectory_id_;
  std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_;

  // Time at which we last logged the rates of incoming sensor data.
  std::chrono::steady_clock::time_point last_logging_time_;
  std::map<std::string, common::RateTimer<>> rate_timers_;

构造函数

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    const proto::TrajectoryBuilderOptions& trajectory_options,
    sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
    const std::set<SensorId>& expected_sensor_ids,
    std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
    : sensor_collator_(sensor_collator),
      collate_landmarks_(trajectory_options.collate_landmarks()),
      collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
      trajectory_id_(trajectory_id),
      wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
      last_logging_time_(std::chrono::steady_clock::now()) {
    
    
  absl::flat_hash_set<std::string> expected_sensor_id_strings;
  for (const auto& sensor_id : expected_sensor_ids) {
    
    
    if (sensor_id.type == SensorId::SensorType::LANDMARK &&
        !collate_landmarks_) {
    
    
      continue;
    }
    if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
        !collate_fixed_frame_) {
    
    
      continue;
    }
    expected_sensor_id_strings.insert(sensor_id.id);
  }
  sensor_collator_->AddTrajectory(
      trajectory_id, expected_sensor_id_strings,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
    
    
        HandleCollatedSensorData(sensor_id, std::move(data));
      });
}

数据走向 AddSensorData -> AddData

  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    
    
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }

数据走向 AddData -> Collator::AddSensorData()

void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
    
    
  sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

Collator : public CollatorInterface

成员变量

  // Queue keys are a pair of trajectory ID and sensor identifier.
  OrderedMultiQueue queue_;

  // Map of trajectory ID to all associated QueueKeys.
  absl::flat_hash_map<int, std::vector<QueueKey>> queue_keys_;

AddTrajectory()

传入回调函数

// cartographer/mapping/internal/collated_trajectory_builder.cc
  sensor_collator_->AddTrajectory(
      trajectory_id, expected_sensor_id_strings,
      [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
    
    
        HandleCollatedSensorData(sensor_id, std::move(data));
      });

void Collator::AddTrajectory(
    const int trajectory_id,
    const absl::flat_hash_set<std::string>& expected_sensor_ids,
    const Callback& callback) {
    
    
  for (const auto& sensor_id : expected_sensor_ids) {
    
    
    const auto queue_key = QueueKey{
    
    trajectory_id, sensor_id};
    queue_.AddQueue(queue_key,
                    [callback, sensor_id](std::unique_ptr<Data> data) {
    
    
                      callback(sensor_id, std::move(data));
                    });
    queue_keys_[trajectory_id].push_back(queue_key);
  }
}

数据走向 Collator::AddSensorData() -> OrderedMultiQueue::Add()

void Collator::AddSensorData(const int trajectory_id,
                             std::unique_ptr<Data> data) {
    
    
  QueueKey queue_key{
    
    trajectory_id, data->GetSensorId()};
  queue_.Add(std::move(queue_key), std::move(data));
}

将数据传入 OrderedMultiQueue 队列中 等待处理

OrderedMultiQueue类

成员变量

  // Used to verify that values are dispatched in sorted order.
  common::Time last_dispatched_time_ = common::Time::min();

  std::map<int, common::Time> common_start_time_per_trajectory_;
  std::map<QueueKey, Queue> queues_;
  QueueKey blocker_;

数据走向 OrderedMultiQueue::Add() -> OrderedMultiQueue::Add()

放入了 BlockingQueue 队列中

void OrderedMultiQueue::Add(const QueueKey& queue_key,
                            std::unique_ptr<Data> data) {
    
    
  auto it = queues_.find(queue_key);
  if (it == queues_.end()) {
    
    
    LOG_EVERY_N(WARNING, 1000)
        << "Ignored data for queue: '" << queue_key << "'";
    return;
  }
  it->second.queue.Push(std::move(data));
  Dispatch();
}

之后调用 Dispatch() 将数据使用 callback() 处理数据

数据走向 OrderedMultiQueue::Queue -> CollatedTrajectoryBuilder::HandleCollatedSensorData()

数据走向 CollatedTrajectoryBuilder::HandleCollatedSensorData() -> GlobalTrajectoryBuilder::AddSensorData()

void CollatedTrajectoryBuilder::HandleCollatedSensorData(
    const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
    
    
  auto it = rate_timers_.find(sensor_id);
  if (it == rate_timers_.end()) {
    
    
    it = rate_timers_
             .emplace(
                 std::piecewise_construct, std::forward_as_tuple(sensor_id),
                 std::forward_as_tuple(
                     common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
             .first;
  }
  it->second.Pulse(data->GetTime());

  if (std::chrono::steady_clock::now() - last_logging_time_ >
      common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
    
    
    for (const auto& pair : rate_timers_) {
    
    
      LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
    }
    last_logging_time_ = std::chrono::steady_clock::now();
  }

  // 传入的是wrapped_trajectory_builder_
  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}

  // 调用传入的trajectory_builder的AddSensorData()
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    
    
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

GlobalTrajectoryBuilder

  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    
    
    if (local_trajectory_builder_) {
    
    
      local_trajectory_builder_->AddImuData(imu_data);
    }
    pose_graph_->AddImuData(trajectory_id_, imu_data);
  }