cartographer_ros node （四）：传感器消息处理

本次主要讲解node.cc文件中传感器信息处理的回调函数主要包括以下函数

• 处理雷达扫描信息

  // 调用SensorBridge的传感器处理函数进行数据处理 void Node::HandleLaserScanMessage(const int trajectory_id,                                   const std::string& sensor_id,                                   const sensor_msgs::LaserScan::ConstPtr& msg) { 
              absl::MutexLock lock(&mutex_);   // 传感器数据是否根据配置跳过？   if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { 
                return;   }   map_builder_bridge_.sensor_bridge(trajectory_id)       ->HandleLaserScanMessage(sensor_id, msg); } 

  这里的Pluse函数是根据处理的数据数量与接受的数据数量之比来判断当前传感器消息是否需要处理的采样器核心函数

• 多回声波雷达消息处理

  // 调用SensorBridge数据处理传感器处理函数 void Node::HandleMultiEchoLaserScanMessage(     const int trajectory_id, const std::string& sensor_id,     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { 
              absl::MutexLock lock(&mutex_);   // 传感器数据是否根据配置跳过？   if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { 
           
      return;
    }
    map_builder_bridge_.sensor_bridge(trajectory_id)
        ->HandleMultiEchoLaserScanMessage(sensor_id, msg);
  }
  

• 点云消息处理

  // 调用SensorBridge的传感器处理函数进行数据处理
  void Node::HandlePointCloud2Message(
      const int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::PointCloud2::ConstPtr& msg) { 
           
    absl::MutexLock lock(&mutex_);
    // 根据配置,是否将传感器数据跳过
    if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { 
           
      return;
    }
    map_builder_bridge_.sensor_bridge(trajectory_id)
        ->HandlePointCloud2Message(sensor_id, msg);
  }
  

• imu消息处理

  	/** * @brief 处理imu数据,imu的数据走向有2个 * 第1个是传入PoseExtrapolator,用于位姿预测与重力方向的确定 * 第2个是传入SensorBridge,使用其传感器处理函数进行imu数据处理 * * @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;
    }
  
    // 将ros消息格式转换为cartographer::sensor::ImuData消息格式
    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);
  }
  

  这里要注意imu消息除了map_builder_bridge_.sensor_bridge外还给到了位姿外推器用于位姿推测和重力方向确定

• 里程计消息处理

  	/** * @brief 处理里程计数据,里程计的数据走向有2个 * 第1个是传入PoseExtrapolator,用于位姿预测 * 第2个是传入SensorBridge,使用其传感器处理函数进行里程计数据处理 * * @param[in] trajectory_id 轨迹id * @param[in] sensor_id 里程计的topic名字 * @param[in] msg 里程计的ros格式的数据 */
  void Node::HandleOdometryMessage(const int trajectory_id,
                                   const std::string& sensor_id,
                                   const nav_msgs::Odometry::ConstPtr& msg) { 
           
    absl::MutexLock lock(&mutex_);
    if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { 
           
      return;
    }
    auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
    auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
    // 使用位姿外推器进行位姿预测
    if (odometry_data_ptr != nullptr) { 
           
      extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
    }
    sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
  }
  

  这里里程计与imu消息一样，也给到了位姿外推器进行位姿预测

• GPS消息处理

  	// 调用SensorBridge的传感器处理函数进行数据处理
  void Node::HandleNavSatFixMessage(const int trajectory_id,
                                    const std::string& sensor_id,
                                    const sensor_msgs::NavSatFix::ConstPtr& msg) { 
           
    absl::MutexLock lock(&mutex_);
    // 根据配置,是否将传感器数据跳过
    if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) { 
           
      return;
    }
    map_builder_bridge_.sensor_bridge(trajectory_id)
        ->HandleNavSatFixMessage(sensor_id, msg);
  }
  

• 路标消息处理

  	// 调用SensorBridge的传感器处理函数进行数据处理
  void Node::HandleLandmarkMessage(
      const int trajectory_id, const std::string& sensor_id,
      const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { 
           
    absl::MutexLock lock(&mutex_);
    // 根据配置,是否将传感器数据跳过
    if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) { 
           
      return;
    }
    map_builder_bridge_.sensor_bridge(trajectory_id)
        ->HandleLandmarkMessage(sensor_id, msg);
  }