Cartographer添加新的传感器接口AddSensorData(

在C++使用addensorData的时候，使用了unique指针的函数指针，暂时不能使用函数函数重载的方式实现。需要使用通过添加新的函数名称的方式实现

Ros端流程图

Ros端的Cartographer添加数据的流程是这样的

获取数据是通过监听topic实现的

在node.cc里面初始化，获取C++端运行结果之后，通过   发布出去

Node::Node(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer,
     wish::pure_loc_cofig*  config_pure  )
    : node_options_(node_options),
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)
 {
  carto::common::MutexLocker lock(&mutex_);
.....................
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
      &Node::PublishTrajectoryNodeList, this));
......................
  }

发布消息

void Node::PublishTrajectoryStates( const ::ros::WallTimerEvent& timer_event ) 
{
  carto::common::MutexLocker lock(&mutex_);
  for ( const auto& entry : map_builder_bridge_.GetTrajectoryStates() )
   {
.......................

C++端添加接口

C++端就比较麻烦

，涉及到众多修改，定义回调函数格式，使用纯虚函数接口因此需要实例化所有的接口函数

需要cartographer/mapping/internal/testing/mock_trajectory_builder.h定义调用函数的格式

  MOCK_METHOD3(AddSensorDataConfine,
               void(const std::string &, const int trajectory_id_confine,  const sensor::TimedPointCloudData &));

基类为抽象类cartographer/sensor/collator_interface.h

virtual void AddSensorDataConfine(int trajectory_id, const int trajectory_id_confine,  std::unique_ptr<Data> data) = 0;

需要在  cartographer/sensor/internal/collator.h覆盖实例化

void AddSensorDataConfine(int trajectory_id, const int trajectory_id_confine,  std::unique_ptr<Data> data) override;

需要在 cartographer/mapping/trajectory_builder_interface.h覆盖实例化

  virtual void AddSensorDataConfine( const std::string& sensor_id,const int trajectory_id_confine,const sensor::TimedPointCloudData& timed_point_cloud_dat) = 0;

需要在  cartographer/mapping/internal/collated_trajectory_builder.h 覆盖实例化

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

需要在  cartographer/cloud/internal/client/trajectory_builder_stub.h 覆盖实例化

  void AddSensorDataConfine(
      const std::string& sensor_id,
          const int trajectory_id_confine,
      const sensor::TimedPointCloudData& timed_point_cloud_dat) override;

需要在 cartographer/sensor/internal/trajectory_collator.h覆盖实例化

void AddSensorDataConfine(int trajectory_id, const int trajectory_id_confine,  std::unique_ptr<Data> data) override;

每一个实例化的头文件，如果使用，写出相应的函数实体

通过层层转接最终使用的函数实体在/cartographer/mapping/internal/global_trajectory_builder.cc

  void AddSensorDataConfine(
      const std::string& sensor_id,const int trajectory_id_confine,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override
  {
# ifdef LOG_FUNC_wish
    { LOG(INFO) << "..GlobalTrajectoryBuilder--AddSensorData (....trajectory_id_confine!"; }
#endif
    CHECK(local_trajectory_builder_)
        << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
......................

至此完成添加新的addSensorData( 接口