C++Node类Cartographer开始轨迹的处理深度详解

"C++Node类Cartographer开始轨迹的处理深度详解"是关于使用C++ Node类库Cartographer中开始轨迹处理的详细攻略。

在Cartographer中，开始轨迹处理包括以下几个步骤：

1.创建一个Cartographer运行时环境

需要使用Cartographer的前提是已经在计算机上安装了Cartographer运行时环境，可以在命令行中输入"cartographer_version"命令检测是否已经安装成功。如果未安装，可以参考Cartographer官方文档进行安装。

2.准备地图、传感器数据和配置文件

在运行Cartographer之前需要准备一个地图、传感器数据和一个配置文件。地图可以是一个CAD或地理信息系统（GIS）文件格式，传感器数据可以是来自GPS、激光雷达或相机的信息，配置文件必须包含Cartographer的参数设置。

3.创建Node类

使用C++实现的Cartographer库是基于ROS（Robot Operating System）系统的，因此需要创建一个Node类来调用Cartographer的API进行数据处理，实现的代码如下：

#include "cartographer_ros/node.h"

namespace cartographer_ros {

class Node {
 public:
  Node(const NodeOptions& node_options,
       std::unique_ptr<cartographer_ros::MapBuilderBridge> map_builder_bridge);
  ~Node();

  Node(const Node&) = delete;
  Node& operator=(const Node&) = delete;

  void Initialize();

  void RunForever();

 private:
  const NodeOptions node_options_;
  std::unique_ptr<cartographer_ros::MapBuilderBridge> map_builder_bridge_;
  std::unique_ptr<cartographer_ros::TimedRosMessageFilter<sensor_msgs::LaserScan>> laser_scan_filter_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster_;
  std::unique_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster_;

  // ... other member variables
};

}  // namespace cartographer_ros

在上面的代码中，我们定义了一个名为Node的类，用于调用Cartographer库中的API进行数据处理。在类中定义了一些成员变量和成员函数。

4.开始轨迹处理

接下来，在C++实现的Cartographer库中开始轨迹处理的代码如下：

namespace cartographer_ros {

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& trajectory_options) {
  // 选择初始化滤波器
  carto::mapping::TrajectoryBuilder::PoseEstimate pose_estimate;
  if (trajectory_options.use_initial_pose) {
    pose_estimate =
        carto::mapping::PoseGraphInterface::GetLatestPoseFromTrajectoryOrZero(
            map_builder_bridge_->GetPoseGrapnInterface(),
            trajectory_options.initial_trajectory_pose_estimate.trajectory_id);
  }
  if (!carto::common::FromRosMessage(trajectory_options.imu_sampling_ratio, &pose_estimate.constant_data->imu_sampling_ratio)) {
    LOG(ERROR) << "Invalid imu_sampling_ratio: " << trajectory_options.imu_sampling_ratio;
    return;
  }

  // 开始轨迹
  const int trajectory_id = map_builder_bridge_->AddTrajectoryBuilder(
      std::unique_ptr<carto::mapping::TrajectoryBuilder>(new carto::mapping::TrajectoryBuilder(
          trajectory_options.trajectory_builder_options,
          map_builder_bridge_->sparse_pose_graph()->GetTrajectoryNodeOptions(
              trajectory_options.trajectory_builder_options.trajectory_builder_3d_options().min_range(),
              trajectory_options.trajectory_builder_options.trajectory_builder_3d_options().max_range(),
              carto::mapping::kImuLinearAccelerationNoiseModel)),
          trajectory_id));
  carto::mapping::TrajectoryBuilderInterface* const trajectory_builder =
      map_builder_bridge_->GetTrajectoryBuilderById(trajectory_id);

  //选择订阅传感器数据
  const std::unordered_set<
      ::cartographer::mapping::TrajectoryBuilderInterface::SensorId,
      ::cartographer::mapping::TrajectoryBuilderInterface::SensorIdHash>
      expected_sensor_ids = trajectory_builder->expected_sensor_ids();

  int message_period_rounding_error_ns = 0;
  if (ContainsKey(expected_sensor_ids, kLaserScanTopic)) {
    // 订阅激光雷达数据
    laser_scan_filter_.reset(new cartographer_ros::TimedRosMessageFilter<sensor_msgs::LaserScan>(
        node_handle_, kLaserScanTopic, message_queue_size_,
        ::ros::Duration(0.2 * pose_estimate.constant_data->imu_gravity_time_constant()),
        ::ros::Duration(kMessageSubscriptionsTimeoutSec),
        [this, trajectory_id](const std::deque<cartographer_ros::TimedMessage<sensor_msgs::LaserScan>>& laser_msg_buffer,
                              const cartographer_ros::Time earliest_timestamp) {
          HandleLaserScanMessage(trajectory_id, laser_msg_buffer, earliest_timestamp);
        }));

    // 准备激光雷达数据处理相关的参数
    carto::sensor::LaserFan3D laser_fan_3d;
    const auto process_laser_scan = [this, trajectory_id, &laser_fan_3d](
                                         const std::string& sensor_id,
                                         const ::cartographer::common::Time time,
                                         Eigen::Vector3f* const origin,
                                         ::cartographer::sensor::PointCloudWithIntensities* const ranges) {
      cartographer_ros_msgs::SensorTopics msg;
      cartographer_ros::SensorBridge(sensor_id, msg).ToRosMessage(time, laser_fan_3d);
      this->HandleRangefinderMessage(trajectory_id, msg, laser_fan_3d.origin, laser_fan_3d.point_cloud,
                                     laser_fan_3d.time);
    };

    // 订阅激光雷达数据以及tf transform
    laser_scan_filter_->connect();
    laser_scan_subscriber_ = node_handle_->subscribe(
        kLaserScanTopic, kSubscriberQueueSize,
        [this, process_laser_scan](const sensor_msgs::LaserScan::ConstPtr& msg) {
          ::cartographer::common::Time timestamp = FromRos(msg->header.stamp);
          // 处理激光雷达数据
          std::deque<cartographer_ros::TimedMessage<sensor_msgs::LaserScan>> timed_msg;
          timed_msg.emplace_back(timestamp, msg);
          laser_scan_filter_->on_new_message(std::move(timed_msg), timestamp);
        });

    if (trajectory_options.use_nav_sat) {
      // 订阅GPS数据
      gps_subscriber_ = node_handle_->subscribe<sensor_msgs::NavSatFix>(
          kGpsFixTopic, kSubscriberQueueSize,
          [this, trajectory_id](const sensor_msgs::NavSatFix::ConstPtr& msg) {
            HandleNavSatFixMessage(trajectory_id, *msg);
          });
    }
  }
}

上面的代码中，我们实现了开始轨迹处理的核心逻辑，包括从传感器订阅数据、处理、转换、建图等操作流程。

示例1：假设我们有一个地图文件"example_map.pgm"和一个传感器数据文件"laser_data.bag"，想要使用Cartographer建立地图。我们可以运行以下命令：

roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=/path/to/laser_data.bag bag_topics:=/scan,config_file:=/path/to/config.lua map_filestem:=example_map

然后，在rviz中打开Cartographer的配置文件"configuration_files/backpack_2d.lua"，选择主题"/map"和"/path"，可以看到通过Cartographer创建的地图。

示例2：使用IMU数据和GPS数据进行建筑物内部地图绘制，配置文件如下：

include "cartographer_ros/configuration_files/backpack_3d.lua"

options = {
  sensor_bridge_options = {
    proteins = {
      imu = {  
        tracked_frame = "imu",
        published_frame = "imu",
        lookup_table = "config_imu.lua" 
      }
    },
    navsat_translator_options = {
      tracking_frame = "gps",
    }
  }
}

return options

使用以下命令开始地图绘制：

roslaunch cartographer_ros backpack_3d.launch bag_filenames:="/path/to/laser_data.bag" bag_topics:="/velodyne_points,/imu/um7,/gps/filtered_imu" configuration_directory:=/path/to/configuration_files

在rviz中打开主题"/map"，可以看到通过Cartographer创建的建筑物地图。

以上就是关于使用C++ Node类库Cartographer中开始轨迹处理的详细攻略，希望能对大家有所帮助。