回调函数

hector_mapping/src/HectorMappingRos.cpp 235 line

void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
{
  //如果暂停扫描，则返回
  if (pause_scan_processing_)
  {
    return;
  }

  if (hectorDrawings)
  {
    hectorDrawings->setTime(scan.header.stamp);
  }

  ros::WallTime start_time = ros::WallTime::now();
  if (!p_use_tf_scan_transformation_)
  {
    // If we are not using the tf tree to find the transform between the base frame and laser frame,
    // then just convert the laser scan to our data container and process the update based on our last
    // pose estimate
    //如果不使用基座标与雷达坐标变换
    //只将雷达扫描信息转好到我们的数据目录并且不断加载基于最新位姿预测的更新
    this->rosLaserScanToDataContainer(scan, laserScanContainer, slamProcessor->getScaleToMap());
    slamProcessor->update(laserScanContainer, slamProcessor->getLastScanMatchPose());
  }
  else
  {
    //如果使用基座标与雷达坐标转换，开始进行坐标变换
    const ros::Duration dur(0.5);
    //定义了一个持续时间 dur=0.5s
    tf::StampedTransform laser_transform;
    //定义位姿变换 laser_transform
    if (tf_.waitForTransform(p_base_frame_, scan.header.frame_id, scan.header.stamp, dur))
    //等待 dur=0.5 时间内，判断是否将 base_frame 转换到 laser_transform
    {
      tf_.lookupTransform(p_base_frame_, scan.header.frame_id, scan.header.stamp, laser_transform);
    }
    else
    {
      //超时无法找到坐标变换
      ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str());
      return;
    }
    //将 雷达数据 转换到 laser_point_cloud
    projector_.projectLaser(scan, laser_point_cloud_, 30.0);

    //如果有任何人订阅这个点云，就将点云数据话题进行发布
    if (scan_point_cloud_publisher_.getNumSubscribers() > 0)
    {
      scan_point_cloud_publisher_.publish(laser_point_cloud_);
    }

    // Convert the point cloud to our data container
    //将 转换的点云数据  laser_point_cloud  抓换到我们需要的点云
    this->rosPointCloudToDataContainer(laser_point_cloud_, laser_transform, laserScanContainer, slamProcessor->getScaleToMap());

    // Now let's choose the initial pose estimate for our slam process update
    //为slam建图更新选择初始位姿预测点
    Eigen::Vector3f start_estimate(Eigen::Vector3f::Zero());
    if (initial_pose_set_)
    //如果使用者有设定的初始位姿
    {
      // User has requested a pose reset
      initial_pose_set_ = false;
      start_estimate = initial_pose_;
    }
    else if (p_use_tf_pose_start_estimate_)
    {
      // Initial pose estimate comes from the tf tree 
      //从 tf 树中选取初始预测位姿
                  //       try {
                  //     逻辑代码块1;
                  // } catch(ExceptionType e) {
                  //     处理代码块1;
                  // }
      try
      {
        tf::StampedTransform stamped_pose;
        //在 dur=0.5s 等待和检查坐标变换
        tf_.waitForTransform(p_map_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
        tf_.lookupTransform(p_map_frame_, p_base_frame_,  scan.header.stamp, stamped_pose);
        //
        const double yaw = tf::getYaw(stamped_pose.getRotation());
        //获得初始位姿预测值，包含x、y
        start_estimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(), stamped_pose.getOrigin().getY(), yaw);
      }
      catch(tf::TransformException e)
      //如果获取位姿出错，将  最新的扫描匹配的位姿 lastScanMatchPose 赋值给 预测位姿
      {
        ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());
        start_estimate = slamProcessor->getLastScanMatchPose();
      }
    }
    else
    {
      // If none of the above, the initial pose is simply the last estimated pose
      //如果上述都没有，初始位姿就是最后预测位姿
      start_estimate = slamProcessor->getLastScanMatchPose();
    }

    // If "p_map_with_known_poses_" is enabled, we assume that start_estimate is precise and doesn't need to be refined
    //如果 p_map_with_known_poses_ 是1，就认为初始位姿预测是精确的，不需要再重新定义
    if (p_map_with_known_poses_)
    {
      //开始对雷达扫描进行更新
      slamProcessor->update(laserScanContainer, start_estimate, true);
    }
    else
    {
      slamProcessor->update(laserScanContainer, start_estimate);
    }
  }

  // If the debug flag "p_timing_output_" is enabled, print how long this last iteration took
  //如果 p_timing_output_ 是 1 ，输出最后一次迭代完成使用多长时间
  if (p_timing_output_)
  {
    ros::WallDuration duration = ros::WallTime::now() - start_time;
    //输出迭代花费多少 ms
    ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec()*1000.0f );
  }

  // If we're just building a map with known poses, we're finished now. Code below this point publishes the localization results.
  //如果是已经知道位姿的情况构造地图，此时已经完成。下列代码发布定位的最终结果
  if (p_map_with_known_poses_)
  //如果已知位姿，则返回 
  {
    return;
  }

//slam加载函数
//获取到最新的扫描匹配位姿   带协方差的位姿
  poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_);

  // Publish pose with and without covariances
  //发布不带协方差的位姿
  poseUpdatePublisher_.publish(poseInfoContainer_.getPoseWithCovarianceStamped());
  posePublisher_.publish(poseInfoContainer_.getPoseStamped());

  // Publish odometry if enabled
  //如果存在里程计，发布里程计
  if(p_pub_odometry_)
  {
    nav_msgs::Odometry tmp;
    //定义里程计为 tmp ，获取里程计的 pose 和 header 
    tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose;
    tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;
    tmp.child_frame_id = p_base_frame_;
    //发布里程计
    odometryPublisher_.publish(tmp);
  }

  // Publish the map->odom transform if enabled
  //如果使能，发布map -> odom 的坐标变换
  if (p_pub_map_odom_transform_)
  {
    tf::StampedTransform odom_to_base;
    try
    {
      //对 odom 和 base_frame 进行坐标变换
      tf_.waitForTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));
      tf_.lookupTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, odom_to_base);
    }
    catch(tf::TransformException e)
    {
      ROS_ERROR("Transform failed during publishing of map_odom transform: %s",e.what());
      odom_to_base.setIdentity();
    }
    //获取map 与 odom坐标变换
    map_to_odom_ = tf::Transform(poseInfoContainer_.getTfTransform() * odom_to_base.inverse());
    tfB_->sendTransform( tf::StampedTransform (map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
  }

  // Publish the transform from map to estimated pose (if enabled)
  //如果使能，发布预测位姿与map的坐标变换
  if (p_pub_map_scanmatch_transform_)
  {
    tfB_->sendTransform( tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
  }
}