微信公众号搜"智元新知"关注
微信扫一扫可直接关注哦！

01cartographer_ros进阶

分类：常见问题作者：qq_44178391

node.cc

node.h和node.cc所包含的内容太多，只能分段解析！

01 构造函数

Node类的初始化是在node_main.cc中的Run函数中构造

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

Node类的构造函数一共完成了四步：

  • Step 1： 声明需要发布的topic

  • Step 2： 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中

  • Step 3： 处理之后的点云的发布器

  • Step 4： 进行定时器与函数的绑定, 定时发布数据

/**
 * @brief
 * 声明ROS的一些topic的发布器, 服务的发布器, 以及将时间驱动的函数与定时器进行绑定
 *
 * @param[in] node_options 配置文件的内容
 * @param[in] map_builder SLAM算法的具体实现
 * @param[in] tf_buffer tf
 * @param[in] collect_metrics 是否启用metrics,默认不启用
 */
Node::Node(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
    : node_options_(node_options),
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
  // 将mutex_上锁, 防止在初始化时数据被更改
  absl::MutexLock lock(&mutex_);

  // 默认不启用
  if (collect_metrics) {
    metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
    carto::metrics::RegisterallMetrics(metrics_registry_.get());
  }

  // Step: 1 声明需要发布的topic

  // 发布SubmapList
  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_pose
  landmark_poses_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
  // 发布约束
  constraint_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kConstraintListTopic, kLatestOnlyPublisherQueueSize);
  // 发布tracked_pose, 默认不发布
  if (node_options_.publish_tracked_pose) {
    tracked_pose_publisher_ =
        node_handle_.advertise<::geometry_msgs::PoseStamped>(
            kTrackedPosetopic, kLatestOnlyPublisherQueueSize);
  }
  // lx add
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    point_cloud_map_publisher_ =
        node_handle_.advertise<sensor_msgs::PointCloud2>(
            kPointCloudMapTopic, kLatestOnlyPublisherQueueSize, true);
  }

  // Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
  service_servers_.push_back(node_handle_.advertiseService(
      kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
  service_servers_.push_back(node_handle_.advertiseService(
      kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, 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));
  service_servers_.push_back(node_handle_.advertiseService(
      kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
  service_servers_.push_back(node_handle_.advertiseService(
      kReadMetricsServiceName, &Node::HandleReadMetrics, this));

  // Step: 3 处理之后的点云的发布器
  scan_matched_point_cloud_publisher_ =
      node_handle_.advertise<sensor_msgs::PointCloud2>(
          kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);

  // Step: 4 进行定时器与函数的绑定, 定时发布数据
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.submap_publish_period_sec),  // 0.3s
      &Node::PublishSubmapList, this));
  if (node_options_.pose_publish_period_sec > 0) {
    publish_local_trajectory_data_timer_ = node_handle_.createTimer(
        ::ros::Duration(node_options_.pose_publish_period_sec),  // 5e-3s
        &Node::PublishLocalTrajectoryData, this);
  }
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(
          node_options_.trajectory_publish_period_sec),  // 30e-3s
      &Node::PublishTrajectoryNodeList, this));
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(
          node_options_.trajectory_publish_period_sec),  // 30e-3s
      &Node::PublishLandmarkPosesList, this));
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kConstraintPublishPeriodSec),  // 0.5s
      &Node::PublishConstraintList, this));
  // lx add
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    wall_timers_.push_back(node_handle_.createWallTimer(
        ::ros::WallDuration(kPointCloudMapPublishPeriodSec),  // 10s
        &Node::PublishPointCloudMap, this));
  }
}

02 开始轨迹

开始轨迹的流程

node_main.cc的Run函数构造了node类，马上利用node类中的成员函数StartTrajectoryWithDefaultTopics开启一条默认的轨迹！

  • 变量start_trajectory_with_default_topics是在node_main.cc中，gflags定义的一些变量中定义的，默认为true
  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

  // 如果加载了pbstream文件, 就执行这个函数
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

在node类中的成员函数StartTrajectoryWithDefaultTopics中

  • 首先检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息，存在返回true，不存在返回false
  • 然后调用node类中的另一个成员函数AddTrajectory 
// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

在node类中的成员函数AddTrajectory中

  • 首先调用node的成员函数ComputeExpectedSensorIds中，根据配置文件, 确定所有需要的topic的名字的集合

    • lua文件中，在订阅的所有激光扫描、多回波激光扫描和点云主题的基础上，如果对应的话题只有一个传感器, 那订阅的topic就是topic；如果是存在多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推；

        num_laser_scans = 0,                      -- 是否使用单线激光数据
  num_multi_echo_laser_scans = 0,           -- 是否使用multi_echo_laser_scans数据
  num_subdivisions_per_laser_scan = 1,      -- 1帧数据被分成几次处理,一般为1
  num_point_clouds = 1,                     -- 是否使用点云数据

    • 3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个

    • odometry（里程计）可有可无, topic的个数只能有一个

    • gps可有可无, topic的个数只能有一个

    • Landmark可有可无, topic的个数只能有一个

  • 然后调用MapBuilderBridge类的成员函数AddTrajectory，传入上一步获得的topic的名字的集合以及TrajectoryOptions的配置参数！
    • 这个函数在之后的MapBuilderBridge类的时候会详细注释
  • 新增一个位姿估计器（imu和里程计的一个融合，它只是去做预测，前端匹配之前的先验！）
    • 以1ms, 以及重力常数10, 作为参数构造PoseExtrapolator
  • 新添加一个传感器采样器
    • 频率为1的时候，表示每一帧点云数据都使用；表示两帧使用一次点云数据
    • 从lua文件中获取自己所设置的各个传感器的频率（默认所有传感器的频率都为1）来进行参数构造TrajectorySensorSamplers
  • 订阅话题与注册回调函数
  • 创建了一个3s执行一次的定时器
    • 由于oneshot=true, 所以只执行一次，检查设置的topic名字是否在ros中存在, 不存在则报错
      （出现以上图片的错误的一个解决办法就是去launch文件重新设置remap即可！）

      报错的情况

  • 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
int Node::AddTrajectory(const TrajectoryOptions& options) {

  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);

  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);

  // 新生成一个传感器数据采样器
  AddSensorSamplers(trajectory_id, options);

  // 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);

  // 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
  // 检查设置的topic名字是否在ros中存在, 不存在则报错
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));

  // 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }

  return trajectory_id;
}

03 传感器数据的走向与传递

在开始轨迹的LaunchSubscribers中，注册了各个传感器的回调函数，当每一帧传感器数据传入时，对应的回调函数就会被调用来处理传感器数据！

传感器 回调函数 传感器数据走向
imu Node::HandleImuMessage 处理imu数据,imu的数据走向有2个：
1）传入PoseExtrapolator，用于位姿预测与重力方向的确定；
2）第2个是传入SensorBridge，使用其传感器处理函数进行imu数据处理
里程计 Node::HandleodometryMessage 处理里程计数据,里程计的数据走向有2个
1）传入PoseExtrapolator，用于位姿预测
2）传入SensorBridge,使用其传感器处理函数进行里程计数据处理
laser_scan Node::HandleLaserScanMessage 调用SensorBridge的传感器处理函数进行数据处理
multi_echo_laser_scans Node::HandleMultiEchoLaserScanMessage 调用SensorBridge的传感器处理函数进行数据处理
point_clouds Node::HandlePointCloud2Message 调用SensorBridge的传感器处理函数进行数据处理
gps Node::HandleNavSatFixMessage 调用SensorBridge的传感器处理函数进行数据处理
landmarks Node::HandleLandmarkMessage 调用SensorBridge的传感器处理函数进行数据处理

可以发现，除了imu和里程计的数据走向多了一个传入PoseExtrapolator，其他传感器都直接传入了SensorBridge类中！

这里传入的PoseExtrapolator和前端的位姿推测器是同一个类下的不同对象，是相互独立的！

这里的PoseExtrapolator是否使用对应了lua文件的options中use_pose_extrapolator参数的设置！

这里以imu传感器的回调函数Node::HandleImuMessage为例

 * @param[in] trajectory_id 轨迹id
 * @param[in] sensor_id imu的topic名字
 * @param[in] msg imu的ros格式的数据
 
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);
}

从代码可以看到，这里的sensor_bridge_ptr是由map_builder_bridge_.sensor_bridge(trajectory_id)得到的

所以在了解SensorBridge这个类之前首先得简单了解MapBuilderBridge这个类

MapBuilderBridge.h

各个传感器回调函数中比较关键的一句：

  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
node.cc中的map_builder_bridge_从何而来？

上面的map_builder_bridge是在node.h中定义的

//GUARDED_BY(mutex_)表示使用时需要上锁
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

初始化是在node的构造函数中

Node::Node(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
    : node_options_(node_options),
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
  // 将mutex_上锁, 防止在初始化时数据被更改
  absl::MutexLock lock(&mutex_);

因此，在node_main.cc构建node时，map_builder_bridge就存在了

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id)的用法？

在MapBuilderBridge.h中有定义了一个成员变量的类型为std::unordered_map<int, std::unique_ptr<SensorBridge>> ，键为int类型，值为std::unique_ptr<SensorBridge>类型

  std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

对应的成员函数

// 获取对应轨迹id的SensorBridge的指针
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
  return sensor_bridges_.at(trajectory_id).get();
}

因此，可以看到，sensor_bridges_.at(trajectory_id)就表示键为trajectory_id所对应的值std::unique_ptr<SensorBridge>，而值是指向SensorBridge类的智能指针，加上get()取原始指针，再将原始指针传入给sensor_bridge_ptr进行后续的传感器数据处理！

之前在开始轨迹中，在node类中的成员函数AddTrajectory中第二步，调用了MapBuilderBridge类的成员函数AddTrajectory

  • 调用MapBuilder类中的成员函数AddTrajectoryBuilder，开始一条新的轨迹，接受返回的轨迹id
  • 为这个新轨迹 添加一个SensorBridge
  • 保存轨迹的参数配置
// 开始一条新轨迹
int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  // Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      // lambda表达式 local_slam_result_callback_
      [this](const int trajectory_id, 
             const ::cartographer::common::Time time,
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        // 保存local slam 的结果数据 5个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });
  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);
  // Step: 2 为这个新轨迹 添加一个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)); // CollatedTrajectoryBuilder
  
  // Step: 3 保存轨迹的参数配置
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

预备知识—— local frame 与 global frame

carographer中存在两个地图坐标系, 分别为global frame与local frame（slam也是这样）

local frame

是表达local slam结果的坐标系, 是固定的坐标系, 不会被回环检测与位姿图优化所更改,

其每一帧位姿间的坐标变换不会改变

global frame

是表达被回环检测与位姿图优化所更改后的坐标系, 当有新的优化结果可用时, 此坐标系与任何其他坐标系之间的转换都会跳变.

它的z轴指向上方, 即重力加速度矢量指向-z方向, 即由加速度计测得的重力分量沿+z方向.

原文地址：https://www.jb51.cc/wenti/3281920.html

版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 dio@foxmail.com 举报，一经查实，本站将立刻删除。

c++开发语言

相关推荐

使用Nvdia显卡驱动自带的录屏功能录制游戏
显卡天梯图2024最新版 显卡性能天梯图2024最新排行榜
显卡天梯图2024最新版，显卡是电脑进行图形处理的重要设备，显卡的性能高低决定了电脑的性能高低。显卡天梯图用数据直观地为我们展示了具体性能数据，让我们可以根据这些参数进行显卡性能的选择，让我们可以获得符合自己使用需求的显卡
初始化电脑时出现问题怎么办 初始化电脑时出现问题解决方法
初始化电脑时出现问题怎么办，可以使用win系统的安装介质，连接电脑后重启，选择从介质中引导，然后点击修复计算机-疑难解答-高级选项-自动修复，等待一段时间就可以了。有些小伙伴在初始化电脑是碰到了问题，下面小编给大家带来了初始化电脑时出现问题解决方法。
todesk远程开机怎么设置 todesk远程开机教程
todesk远程开机怎么设置，两台电脑要在同一局域网内，然后需要修改BIOS，还要修改电源选项。我们在使用todesk软件时，有些小伙伴想要进行远程开机，那应该怎么设置呢，下面小编就给大家带来了todesk远程开机教程，一起来看看吧。
油猴谷歌插件怎么安装 油猴谷歌插件安装教程
油猴谷歌插件怎么安装，可以通过谷歌应用商店进行安装，需要才能打开应用商店，或者是下载插件到本地，解压后把Tampermonkey.crx文件拖到谷歌浏览器的程序扩展页面进行安装。小伙伴想知道谷歌浏览器怎么安装油猴，下面小编给大家带来了油猴谷歌插件安装教程。
16g虚拟内存设置多少合适 16g虚拟内存最佳设置方法
虚拟内存这个名词想必很多人都听说过，我们在使用电脑的时候，特别是玩大型游戏时候，如果内存空间不足则会开启使用虚拟内存，这样就能够防止死机的情况。有不少的新用户在安装系统的时候，不知道16G的内存设置多大的虚拟内存，下面小编就详细给大家讲解下多少才
win11本地账户怎么改名 win11本地账户改名图文介绍
win11本地账户怎么改名?win11很多操作都变了样，用户如果想要修改用户名称可能找不到具体的位置在哪。win11系统本地账户的名称都是默认的，很多用户为了有特征性想要更改一些有趣的名称，这样看起来更有个性点。win11本地账户怎么改名，我们可以通过打开控制面板，然后
网速怎么看快慢k/s,b/s是什么意思 网速怎么看快慢k/s,b/s哪个快
网速怎么看快慢k s,b s是什么意思，k b就是网速1kb每秒，b s就是网速1b每秒,1kb=1024b。很多小伙伴在下载或上传文件时都会看着kb s，b s等等单位，这些单位是什么意思呢，哪个比较快呢，下面小编给大家带来了网速怎么看快慢k s,b s哪个快的相关资讯。
笔记本声卡驱动怎么更新 笔记本声卡驱动更新方法
我们在使用电脑的时候，经常需要对各种驱动进行更新，如果长期不更新会导致无法正常的使用。有不少的用户在问，该怎么更新声卡驱动？其实很简单，我们右键“我的电脑”打开“设备管理器”，找到“声音”选项并右键“realtek audio”更新驱动程序即可，具体如下。
应用程序无法正常启动0x0000142怎么办 0x0000142错误代码解决办法
应用程序无法正常启动0x0000142怎么办?我们的电脑在日经月累的使用下，难免会出现各种问题，最近有小伙伴反应自己的电脑出现应用程序无法正常启动0x0000142的错误代码，这要怎么解决呢?下面小编就告诉大家电脑遇到0x0000142错误代码的解决办法。
苹果市值2025年有望达4万亿美元
pythonJavaScriptjavaHTMLPHPreactjsC#AndroidCSSNode.jssqlrpython-3.xMysqLjQueryc++pandasFlutterangularIOSdjangolinuxswifttypescript路由器JSON路由器设置无线路由器h3c华三华三路由器设置华三路由器电脑软件教程arraysdocker软件图文教程Cvue.jslaravelspring-boot