Autonomous_Exploration_Development_Environment的学习笔记(五)

Autonomous_Exploration_Development_Environment的学习笔记(五)

本教程主要完成vehicle_simulation和visualizationTools的程序解析，这是该序列教程的第五篇。由于最近楼主的事情较多，更新的较慢，有感兴趣的同学留言，楼主可以根据你们的需要进行后面内容的更新。

1.vehicle_simulation的参数解析

下面是vehicle_simulation的参数，具体含义见下表

bool use_gazebo_time = false; // 使用仿真时间
double cameraOffsetZ = 0; // 相机z轴方向偏离距离
double sensorOffsetX = 0; // 雷达传感器x轴方向偏离距离
double sensorOffsetY = 0; // 雷达传感器y轴方向偏离距离
double vehicleHeight = 0.75; // 车辆的高度
double terrainVoxelSize = 0.05; // 地形体素网格大小
double groundHeightThre = 0.1; // 地面高度距离阈值
bool adjustZ = false; // Z轴调整标志，主要用于地形识别后的平滑
double terrainRadiusZ = 0.5; // z轴上车辆周围的阈值
int minTerrainPointNumZ = 10; // 最小的z轴地面点数
double smoothRateZ = 0.2; // 控制高度方向平滑的速率
bool adjustIncl = false; // 调整地面点云倾斜的标志
double terrainRadiusIncl = 1.5; // 地面点云倾斜角度的阈值
int minTerrainPointNumIncl = 500; // 地面点云倾斜调整的最小点数阈值
double smoothRateIncl = 0.2; // 倾斜角度平滑的速率
double InclFittingThre = 0.2; // I倾斜角度滤波的阈值
double maxIncl = 30.0; // 最大的倾斜角度
const int systemDelay = 5; // 系统延迟时间
int systemInitCount = 0; // 系统初始化计数
bool systemInited = false; // 系统初始化标志
pcl::PointCloud<pcl::PointXYZI>::Ptr scanData(new pcl::PointCloud<pcl::PointXYZI>()); // 激光雷达的扫描点云

pcl::PointCloud<pcl::PointXYZI>::Ptr terrainCloud(new pcl::PointCloud<pcl::PointXYZI>()); // 地面点云

pcl::PointCloud<pcl::PointXYZI>::Ptr terrainCloudIncl(new pcl::PointCloud<pcl::PointXYZI>()); // 地面倾斜点云

pcl::PointCloud<pcl::PointXYZI>::Ptr terrainCloudDwz(new pcl::PointCloud<pcl::PointXYZI>()); // 地面点云下采样

std::vector<int> scanInd; // 扫描的ID号

ros::Time odomTime; // 时间

float vehicleX = 0; // 车辆x轴起始位置
float vehicleY = 0; // 车辆y轴起始位置
float vehicleZ = 0; // 车辆z轴起始位置
float vehicleRoll = 0; // 车辆Roll角
float vehiclePitch = 0; // 车辆Pitch角
float vehicleYaw = 0; // 车辆Yaw角
float vehicleYawRate = 0; // 车辆Yaw角变化率
float vehicleSpeed = 0; // 车辆速度

float terrainZ = 0; // 地面z轴高度
float terrainRoll = 0; // 地面Roll角
float terrainPitch = 0; // 地面Pitch角

2.vehicle_simulation回调函数的具体解析

总共有3个回调函数，下面具体说明每个函数的具体功能。

2.1 scanHandler函数

void scanHandler(const sensor_msgs::PointCloud2::ConstPtr& scanIn)函数主要是处理gazebo中发布出来的点云信息，用于处理激光扫描数据并进行坐标变换，最后发布变换后的激光扫描消息

void scanHandler(const sensor_msgs::PointCloud2::ConstPtr& scanIn)

{
// 当系统未初始化
if (!systemInited) {
	systemInitCount++;
	// 当计数延迟大于系统延迟时
	if (systemInitCount > systemDelay) {
		systemInited = true;
	}
	return;
}

  

double scanTime = scanIn->header.stamp.toSec();
// 当里程计信息不可用
if (odomSendIDPointer < 0)
{
	return;
}

/*循环处理里程计数据
*通过循环找到时间戳最近的正确的里程计信息
*/
while (odomTimeStack[(odomRecIDPointer + 1) % stackNum] < scanTime &&
odomRecIDPointer != (odomSendIDPointer + 1) % stackNum)
{
	odomRecIDPointer = (odomRecIDPointer + 1) % stackNum;
}

// 获得里程计信息和姿态
double odomRecTime = odomTime.toSec();
float vehicleRecX = vehicleX;
float vehicleRecY = vehicleY;
float vehicleRecZ = vehicleZ;
float vehicleRecRoll = vehicleRoll;
float vehicleRecPitch = vehiclePitch;
float vehicleRecYaw = vehicleYaw;
float terrainRecRoll = terrainRoll;
float terrainRecPitch = terrainPitch;

// 使用仿真时间则从栈中获得时间
if (use_gazebo_time)
{
odomRecTime = odomTimeStack[odomRecIDPointer];
vehicleRecX = vehicleXStack[odomRecIDPointer];
vehicleRecY = vehicleYStack[odomRecIDPointer];
vehicleRecZ = vehicleZStack[odomRecIDPointer];
vehicleRecRoll = vehicleRollStack[odomRecIDPointer];
vehicleRecPitch = vehiclePitchStack[odomRecIDPointer];
vehicleRecYaw = vehicleYawStack[odomRecIDPointer];
terrainRecRoll = terrainRollStack[odomRecIDPointer];
terrainRecPitch = terrainPitchStack[odomRecIDPointer];
}

  

float sinTerrainRecRoll = sin(terrainRecRoll);
float cosTerrainRecRoll = cos(terrainRecRoll);
float sinTerrainRecPitch = sin(terrainRecPitch);
float cosTerrainRecPitch = cos(terrainRecPitch);

scanData->clear();
pcl::fromROSMsg(*scanIn, *scanData);
pcl::removeNaNFromPointCloud(*scanData, *scanData, scanInd);


// 坐标系转换
int scanDataSize = scanData->points.size();
for (int i = 0; i < scanDataSize; i++)
{
// 将lidar的数据绕x轴进行转动
float pointX1 = scanData->points[i].x;
float pointY1 = scanData->points[i].y * cosTerrainRecRoll - scanData->points[i].z * sinTerrainRecRoll;
float pointZ1 = scanData->points[i].y * sinTerrainRecRoll + scanData->points[i].z * cosTerrainRecRoll;


// 将lidar的数据绕y轴进行转动地面Roll角
float pointX2 = pointX1 * cosTerrainRecPitch + pointZ1 * sinTerrainRecPitch;
float pointY2 = pointY1;
float pointZ2 = -pointX1 * sinTerrainRecPitch + pointZ1 * cosTerrainRecPitch;

// 将lidar数据平移车辆的位置转动地面Pitch角
float pointX3 = pointX2 + vehicleRecX;
float pointY3 = pointY2 + vehicleRecY;
float pointZ3 = pointZ2 + vehicleRecZ;

scanData->points[i].x = pointX3;
scanData->points[i].y = pointY3;
scanData->points[i].z = pointZ3;
}

  

// 发布5Hz雷达信息
sensor_msgs::PointCloud2 scanData2;
pcl::toROSMsg(*scanData, scanData2);
scanData2.header.stamp = ros::Time().fromSec(odomRecTime);
scanData2.header.frame_id = "map";
pubScanPointer->publish(scanData2);
}

2.2 terrainCloudHandler函数

terrainCloudHandler函数对地形点云数据进行处理，估计地形的高度和倾斜角度参数，并应用平滑处理以提高估计的精度。这种处理通常用于机器人的导航和感知系统中，以确保地形参数在不断变化的环境中保持准确

void terrainCloudHandler(const sensor_msgs::PointCloud2ConstPtr& terrainCloud2)
{
  // 是否需要对地形参数进行调整
  if (!adjustZ && !adjustIncl)
  {
    return;
  }

  // 读取地面点云
  terrainCloud->clear();
  pcl::fromROSMsg(*terrainCloud2, *terrainCloud);

  pcl::PointXYZI point;
  terrainCloudIncl->clear();
  int terrainCloudSize = terrainCloud->points.size();
  double elevMean = 0;
  int elevCount = 0;
  bool terrainValid = true;
  for (int i = 0; i < terrainCloudSize; i++)
  {
    point = terrainCloud->points[i];

    float dis = sqrt((point.x - vehicleX) * (point.x - vehicleX) + (point.y - vehicleY) * (point.y - vehicleY));

    // 点云距离是否小于地面高度区域阈值
    if (dis < terrainRadiusZ)
    {
      // 点云高度是否小于地面高度阈值
      if (point.intensity < groundHeightThre)
      {
        elevMean += point.z;
        elevCount++;
      }
      else
      {
        terrainValid = false;
      }
    }

    // 距离是否小于地面倾斜判定范围阈值 或者 点云的高度小于地面的阈值
    if (dis < terrainRadiusIncl && point.intensity < groundHeightThre)
    {
      terrainCloudIncl->push_back(point);
    }
  }

  if (elevCount >= minTerrainPointNumZ)
    elevMean /= elevCount;
  else
    terrainValid = false;

  // 如果地形有效且需要调整高度 
  if (terrainValid && adjustZ)
  {
    // 线性化平滑地面z轴高度
    terrainZ = (1.0 - smoothRateZ) * terrainZ + smoothRateZ * elevMean;
  }

  // 对车辆周围的地面点云进行下采样
  terrainCloudDwz->clear();
  terrainDwzFilter.setInputCloud(terrainCloudIncl);
  terrainDwzFilter.filter(*terrainCloudDwz);
  int terrainCloudDwzSize = terrainCloudDwz->points.size();

  // 是否有足够数量的内点（有效的地形点）用于进行倾斜角度的估计 或者  地面无效
  if (terrainCloudDwzSize < minTerrainPointNumIncl || !terrainValid)
  {
    return;
  }

  /*迭代 RANSAC 进行地形参数估计：
   *通过 RANSAC 算法估计地形的俯仰和横滚角度。
   *RANSAC 是一种随机抽样一致性算法，用于估计模型参数（在这里是俯仰和横滚角度），并剔除异常值。
   *迭代次数为5次，每次迭代中计算估计参数 matX。
  **/ 
  cv::Mat matA(terrainCloudDwzSize, 2, CV_32F, cv::Scalar::all(0));
  cv::Mat matAt(2, terrainCloudDwzSize, CV_32F, cv::Scalar::all(0));
  cv::Mat matAtA(2, 2, CV_32F, cv::Scalar::all(0));
  cv::Mat matB(terrainCloudDwzSize, 1, CV_32F, cv::Scalar::all(0));
  cv::Mat matAtB(2, 1, CV_32F, cv::Scalar::all(0));
  cv::Mat matX(2, 1, CV_32F, cv::Scalar::all(0));

  /*这段代码的目的是执行一个 RANSAC（随机抽样一致性）迭代，通过拟合地形点云数据
   *来估计地形的俯仰和横滚参数，并识别和剔除异常值。在每次迭代中，根据当前的地形参数
   *（terrainPitch 和 terrainRoll）计算残差，如果残差大于阈值，则将该点标记为异
   *常值，然后在下一次迭代中将其忽略，直到满足停止条件（5次迭代或没有新的异常值）。最
   *终，matX 中的 terrainPitch 和 terrainRoll 参数将是拟合的结果。这种方法可以提
   *高地形参数的估计精度，并剔除地形点云中的异常值。
  **/
  int inlierNum = 0;
  matX.at<float>(0, 0) = terrainPitch;
  matX.at<float>(1, 0) = terrainRoll;
  for (int iterCount = 0; iterCount < 5; iterCount++)
  {
    int outlierCount = 0;
    for (int i = 0; i < terrainCloudDwzSize; i++)
    {
      point = terrainCloudDwz->points[i];

      matA.at<float>(i, 0) = -point.x + vehicleX;
      matA.at<float>(i, 1) = point.y - vehicleY;
      matB.at<float>(i, 0) = point.z - elevMean;

      if (fabs(matA.at<float>(i, 0) * matX.at<float>(0, 0) + matA.at<float>(i, 1) * matX.at<float>(1, 0) -
               matB.at<float>(i, 0)) > InclFittingThre &&
          iterCount > 0)
      {
        matA.at<float>(i, 0) = 0;
        matA.at<float>(i, 1) = 0;
        matB.at<float>(i, 0) = 0;
        outlierCount++;
      }
    }

    /*这段代码的主要目的是通过拟合地形点云数据来估计地形的俯仰和横滚角度，并在
     *一些条件下判断地形是否有效。如果地形有效，它还会对地形角度进行平滑处理，以
     *减小估计的误差。这样可以提高地形参数的估计精度。
     */
    cv::transpose(matA, matAt);
    matAtA = matAt * matA;
    matAtB = matAt * matB;
    cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); // 使用QR分解求解线性方程组

    if (inlierNum == terrainCloudDwzSize - outlierCount)
      break;
    inlierNum = terrainCloudDwzSize - outlierCount;
  }

  /*这些条件用于对估计的地形参数进行限制和检查，以确保地形估计的合理性和准确性。
   *内点的数量是否小于指定的最小地形点数
   *估计的俯仰角度是否超过了指定的最大俯仰角度
   *估计的横滚角度是否超过了指定的最大横滚角度
   */
  if (inlierNum < minTerrainPointNumIncl || fabs(matX.at<float>(0, 0)) > maxIncl * PI / 180.0 ||
      fabs(matX.at<float>(1, 0)) > maxIncl * PI / 180.0)
  {
    terrainValid = false;
  }

  /*这段代码用于更新估计得到的地形俯仰角度terrainPitch 和横滚角度 terrainRoll，并且应用了平滑处理 */
  if (terrainValid && adjustIncl)
  {
    terrainPitch = (1.0 - smoothRateIncl) * terrainPitch + smoothRateIncl * matX.at<float>(0, 0);
    terrainRoll = (1.0 - smoothRateIncl) * terrainRoll + smoothRateIncl * matX.at<float>(1, 0);
  }
}

2.3 speedHandler函数

void speedHandler(const geometry_msgs::TwistStamped::ConstPtr& speedIn)
{
  vehicleSpeed = speedIn->twist.linear.x;    // 获取速度
  vehicleYawRate = speedIn->twist.angular.z; // 获取角速度
}

3.vehicle_simulation的main函数解析

主函数的功能是：

1. 初始化ROS节点和节点句柄（NodeHandle）。
2. 从ROS参数服务器（Parameter Server）获取配置参数，例如是否使用Gazebo时间（use_gazebo_time）、相机偏移（cameraOffsetZ）、传感器偏移（sensorOffsetX和sensorOffsetY）、车辆高度（vehicleHeight）、车辆位置（vehicleX、vehicleY、vehicleZ）、地形高度（terrainZ）、车辆航向角（vehicleYaw）、地形点云滤波参数等。
3. 创建ROS订阅器（Subscribers）和发布器（Publishers）用于接收和发布传感器数据、车辆状态和仿真模型状态。
4. 初始化地形点云滤波器（terrainDwzFilter）。
5. 进入主循环，其中包含以下主要功能：
   • 更新车辆的姿态和位置，模拟车辆运动。
   • 发布车辆的里程计数据（Odometry）。
   • 发布车辆的TF变换（Transforms）。
   • 发布Gazebo模型状态信息，用于Gazebo仿真。
   • 检查ROS节点是否继续运行，然后使用 ros::spinOnce() 处理传感器数据。
     该节点通过读取参数、订阅传感器数据和发布车辆状态信息，模拟了一个车辆在仿真环境中的运动，并根据传感器数据生成车辆状态信息。这些信息可以用于导航和控制等应用。

4.visualizationTools 的参数解析

下面是visualizationTools的参数，具体含义见下表

string metricFile;                      // 输出探索空间的各项参数的文件路径
string trajFile;                        // 输出探索轨迹的文件路径
string mapFile;                         // 输入地图信息文件的路径
double overallMapVoxelSize = 0.5;       // 表示总体地图体素的尺寸
double exploredAreaVoxelSize = 0.3;     // 表示探索空间体素的尺寸
double exploredVolumeVoxelSize = 0.5;   // 表示探索体积提速的尺寸
double transInterval = 0.2;             // 表示探索时间间隔
double yawInterval = 10.0;              // 表示航向时间间隔
int overallMapDisplayInterval = 2;      // 全局地图显示时间间隔
int overallMapDisplayCount = 0;         // 全局地图显示计数
int exploredAreaDisplayInterval = 1;    // 探索空间显示时间间隔
int exploredAreaDisplayCount = 0;       // 探索空间显示计数

pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloud(new pcl::PointCloud<pcl::PointXYZI>());             // lidar点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr overallMapCloud(new pcl::PointCloud<pcl::PointXYZ>());          // 全局地图点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr overallMapCloudDwz(new pcl::PointCloud<pcl::PointXYZ>());       // 全局地图点云下采样数据
pcl::PointCloud<pcl::PointXYZI>::Ptr exploredAreaCloud(new pcl::PointCloud<pcl::PointXYZI>());      // 探索空间点云数据
pcl::PointCloud<pcl::PointXYZI>::Ptr exploredAreaCloud2(new pcl::PointCloud<pcl::PointXYZI>());     // 探索空间点云数据2
pcl::PointCloud<pcl::PointXYZI>::Ptr exploredVolumeCloud(new pcl::PointCloud<pcl::PointXYZI>());    // 探索空间体积点云
pcl::PointCloud<pcl::PointXYZI>::Ptr exploredVolumeCloud2(new pcl::PointCloud<pcl::PointXYZI>());   // 探索空间体积点云2
pcl::PointCloud<pcl::PointXYZI>::Ptr trajectory(new pcl::PointCloud<pcl::PointXYZI>());             // 轨迹

const int systemDelay = 5;              // 系统时间延时
int systemDelayCount = 0;               // 系统延时计数
bool systemDelayInited = false;         // 系统延时初始化
double systemTime = 0;                  // 系统时间
double systemInitTime = 0;              // 系统初始化时间
bool systemInited = false;              // 系统初始化标志

float vehicleYaw = 0;
float vehicleX = 0, vehicleY = 0, vehicleZ = 0;
float exploredVolume = 0, travelingDis = 0, runtime = 0, timeDuration = 0;

pcl::VoxelGrid<pcl::PointXYZ> overallMapDwzFilter;
pcl::VoxelGrid<pcl::PointXYZI> exploredAreaDwzFilter;
pcl::VoxelGrid<pcl::PointXYZI> exploredVolumeDwzFilter;

sensor_msgs::PointCloud2 overallMap2;

ros::Publisher *pubExploredAreaPtr = NULL;
ros::Publisher *pubTrajectoryPtr = NULL;
ros::Publisher *pubExploredVolumePtr = NULL;
ros::Publisher *pubTravelingDisPtr = NULL;
ros::Publisher *pubTimeDurationPtr = NULL;

FILE *metricFilePtr = NULL;
FILE *trajFilePtr = NULL;

5.visualizationTools 的回调函数解析

visualizationTools总共有3个回调函数,因为不是很复杂，所以就直接注释了，下面具体说明每个函数的具体功能。

void odometryHandler(const nav_msgs::Odometry::ConstPtr &odom)
{
  systemTime = odom->header.stamp.toSec();

  double roll, pitch, yaw;
  geometry_msgs::Quaternion geoQuat = odom->pose.pose.orientation;
  tf::Matrix3x3(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w)).getRPY(roll, pitch, yaw);

  // 控制航向角为[-pi, pi]之间
  float dYaw = fabs(yaw - vehicleYaw);
  if (dYaw > PI)
    dYaw = 2 * PI - dYaw;

  float dx = odom->pose.pose.position.x - vehicleX;
  float dy = odom->pose.pose.position.y - vehicleY;
  float dz = odom->pose.pose.position.z - vehicleZ;
  float dis = sqrt(dx * dx + dy * dy + dz * dz);

  if (!systemDelayInited)
  {
    vehicleYaw = yaw;
    vehicleX = odom->pose.pose.position.x;
    vehicleY = odom->pose.pose.position.y;
    vehicleZ = odom->pose.pose.position.z;
    return;
  }

  if (systemInited)
  {
    timeDuration = systemTime - systemInitTime;

    std_msgs::Float32 timeDurationMsg;
    timeDurationMsg.data = timeDuration;
    pubTimeDurationPtr->publish(timeDurationMsg);
  }

  // 若车辆运动距离小于阈值或者角度小于阈值，则判定车辆没运动，结束回调
  if (dis < transInterval && dYaw < yawInterval)
  {
    return;
  }

  if (!systemInited)
  {
    dis = 0;
    systemInitTime = systemTime;
    systemInited = true;
  }

  travelingDis += dis;

  vehicleYaw = yaw;
  vehicleX = odom->pose.pose.position.x;
  vehicleY = odom->pose.pose.position.y;
  vehicleZ = odom->pose.pose.position.z;

  // 输出以下参数，具体路径看launch中关于trajFilePtr的取值
  fprintf(trajFilePtr, "%f %f %f %f %f %f %f\n", vehicleX, vehicleY, vehicleZ, roll, pitch, yaw, timeDuration);

  pcl::PointXYZI point;
  point.x = vehicleX;
  point.y = vehicleY;
  point.z = vehicleZ;
  point.intensity = travelingDis;
  trajectory->push_back(point);

  // 发布运动轨迹
  sensor_msgs::PointCloud2 trajectory2;
  pcl::toROSMsg(*trajectory, trajectory2);
  trajectory2.header.stamp = odom->header.stamp;
  trajectory2.header.frame_id = "map";
  pubTrajectoryPtr->publish(trajectory2);
}

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudIn)
{
  if (!systemDelayInited)
  {
    systemDelayCount++;
    if (systemDelayCount > systemDelay)
    {
      systemDelayInited = true;
    }
  }

  if (!systemInited)
  {
    return;
  }

  /*这段代码的目的是将接收到的激光雷达点云数据添加到探测体积的点云中，
   *然后对点云进行滤波处理，最后计算探测体积的大小。这个大小可以用于评
   *估探测任务的进展和覆盖区域的大小。
   */
  laserCloud->clear();
  pcl::fromROSMsg(*laserCloudIn, *laserCloud);

  *exploredVolumeCloud += *laserCloud;

  exploredVolumeCloud2->clear();
  exploredVolumeDwzFilter.setInputCloud(exploredVolumeCloud);
  exploredVolumeDwzFilter.filter(*exploredVolumeCloud2);

  pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud = exploredVolumeCloud;
  exploredVolumeCloud = exploredVolumeCloud2;
  exploredVolumeCloud2 = tempCloud;

  // 计算探索过的空间体积
  exploredVolume = exploredVolumeVoxelSize * exploredVolumeVoxelSize * exploredVolumeVoxelSize * exploredVolumeCloud->points.size();


  /*这段代码的作用是定期发布已滤波和更新的探测区域点云消息，以便在可视化工具中查看实时探测区域的变化。*/
  *exploredAreaCloud += *laserCloud;

  exploredAreaDisplayCount++;
  if (exploredAreaDisplayCount >= 5 * exploredAreaDisplayInterval)
  {
    exploredAreaCloud2->clear();
    exploredAreaDwzFilter.setInputCloud(exploredAreaCloud);
    exploredAreaDwzFilter.filter(*exploredAreaCloud2);

    tempCloud = exploredAreaCloud;
    exploredAreaCloud = exploredAreaCloud2;
    exploredAreaCloud2 = tempCloud;

    sensor_msgs::PointCloud2 exploredArea2;
    pcl::toROSMsg(*exploredAreaCloud, exploredArea2);
    exploredArea2.header.stamp = laserCloudIn->header.stamp;
    exploredArea2.header.frame_id = "map";
    pubExploredAreaPtr->publish(exploredArea2);

    exploredAreaDisplayCount = 0;
  }

  // 输出上述参数，具体文件路径看launch中关于metricFilePtr的值
  fprintf(metricFilePtr, "%f %f %f %f\n", exploredVolume, travelingDis, runtime, timeDuration);

  // 发布探索体积
  std_msgs::Float32 exploredVolumeMsg;
  exploredVolumeMsg.data = exploredVolume;
  pubExploredVolumePtr->publish(exploredVolumeMsg);

  // 发布探索的行驶距离
  std_msgs::Float32 travelingDisMsg;
  travelingDisMsg.data = travelingDis;
  pubTravelingDisPtr->publish(travelingDisMsg);
}

void runtimeHandler(const std_msgs::Float32::ConstPtr &runtimeIn)
{ 
  // 读取时间参数
  runtime = runtimeIn->data;
}

6.visualizationTools的mian函数讲解

同样，main函数也是很简单，所以就直接贴注释了

int main(int argc, char **argv)
{
  ros::init(argc, argv, "visualizationTools");
  ros::NodeHandle nh;
  ros::NodeHandle nhPrivate = ros::NodeHandle("~");

  nhPrivate.getParam("metricFile", metricFile);
  nhPrivate.getParam("trajFile", trajFile);
  nhPrivate.getParam("mapFile", mapFile);
  nhPrivate.getParam("overallMapVoxelSize", overallMapVoxelSize);
  nhPrivate.getParam("exploredAreaVoxelSize", exploredAreaVoxelSize);
  nhPrivate.getParam("exploredVolumeVoxelSize", exploredVolumeVoxelSize);
  nhPrivate.getParam("transInterval", transInterval);
  nhPrivate.getParam("yawInterval", yawInterval);
  nhPrivate.getParam("overallMapDisplayInterval", overallMapDisplayInterval);
  nhPrivate.getParam("exploredAreaDisplayInterval", exploredAreaDisplayInterval);

  ros::Subscriber subOdometry = nh.subscribe<nav_msgs::Odometry>("/state_estimation", 5, odometryHandler);

  ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/registered_scan", 5, laserCloudHandler);

  ros::Subscriber subRuntime = nh.subscribe<std_msgs::Float32>("/runtime", 5, runtimeHandler);

  ros::Publisher pubOverallMap = nh.advertise<sensor_msgs::PointCloud2>("/overall_map", 5);

  ros::Publisher pubExploredArea = nh.advertise<sensor_msgs::PointCloud2>("/explored_areas", 5);
  pubExploredAreaPtr = &pubExploredArea;

  ros::Publisher pubTrajectory = nh.advertise<sensor_msgs::PointCloud2>("/trajectory", 5);
  pubTrajectoryPtr = &pubTrajectory;

  ros::Publisher pubExploredVolume = nh.advertise<std_msgs::Float32>("/explored_volume", 5);
  pubExploredVolumePtr = &pubExploredVolume;

  ros::Publisher pubTravelingDis = nh.advertise<std_msgs::Float32>("/traveling_distance", 5);
  pubTravelingDisPtr = &pubTravelingDis;

  ros::Publisher pubTimeDuration = nh.advertise<std_msgs::Float32>("/time_duration", 5);
  pubTimeDurationPtr = &pubTimeDuration;

  overallMapDwzFilter.setLeafSize(overallMapVoxelSize, overallMapVoxelSize, overallMapVoxelSize);
  exploredAreaDwzFilter.setLeafSize(exploredAreaVoxelSize, exploredAreaVoxelSize, exploredAreaVoxelSize);
  exploredVolumeDwzFilter.setLeafSize(exploredVolumeVoxelSize, exploredVolumeVoxelSize, exploredVolumeVoxelSize);

  // 读入地图数据
  pcl::PLYReader ply_reader;
  if (ply_reader.read(mapFile, *overallMapCloud) == -1)
  {
    printf("\nCouldn't read pointcloud.ply file.\n\n");
  }

  // 对地图点云降采样
  overallMapCloudDwz->clear();
  overallMapDwzFilter.setInputCloud(overallMapCloud);
  overallMapDwzFilter.filter(*overallMapCloudDwz);
  overallMapCloud->clear();

  pcl::toROSMsg(*overallMapCloudDwz, overallMap2);

  // 读取系统时间，设置string，并添加到上述输出的文件名中
  time_t logTime = time(0);
  tm *ltm = localtime(&logTime);
  string timeString = to_string(1900 + ltm->tm_year) + "-" + to_string(1 + ltm->tm_mon) + "-" + to_string(ltm->tm_mday) + "-" +
                      to_string(ltm->tm_hour) + "-" + to_string(ltm->tm_min) + "-" + to_string(ltm->tm_sec);

  metricFile += "_" + timeString + ".txt";
  trajFile += "_" + timeString + ".txt";
  metricFilePtr = fopen(metricFile.c_str(), "w");
  trajFilePtr = fopen(trajFile.c_str(), "w");

  ros::Rate rate(100);
  bool status = ros::ok();
  while (status)
  {
    ros::spinOnce();

    // 通过控制overallMapDisplayCount的次数进行overallMap2的更新发布
    overallMapDisplayCount++;
    if (overallMapDisplayCount >= 100 * overallMapDisplayInterval)
    {
      overallMap2.header.stamp = ros::Time().fromSec(systemTime);
      overallMap2.header.frame_id = "map";
      pubOverallMap.publish(overallMap2);

      overallMapDisplayCount = 0;
    }

    status = ros::ok();
    rate.sleep();
  }

  fclose(metricFilePtr);
  fclose(trajFilePtr);

  printf("\nExploration metrics and vehicle trajectory are saved in 'src/vehicle_simulator/log'.\n\n");

  return 0;
}