热门标签
热门文章
- 1高并发高性能接口中，异步打印并采集业务日志的实现方案
- 2Android Launcher 应用程序
- 3org.apache.zookeeper.KeeperException$NoNodeException: KeeperErrorCode = NoNode for
- 4【云开发】小程序端操作数据库详解_小程序数据库
- 510个综合管理系统类开源项目，速速拿去练手_统一用户管理系统开源设计
- 6HTML页面本地正常，部署到服务器稍微异常解决方案_ie 前端样式本地可以,部署测试环境失效
- 7git IDEA的分支合并，merge和rebase的区别_idea merge和rebase
- 8WOKWI - Arduino/ESP32/树莓派等在线Pico仿真学习平台_wokwi仿真平台
- 9鸿蒙系统：华为打造的全新智能生态
- 10Google语音的使用_谷歌tts离线语音包
当前位置: article > 正文
CMU-Exploration 代码笔记（二）
作者：菜鸟追梦旅行 | 2024-04-06 12:01:49
赞
踩
CMU-Exploration 代码笔记（二）
功能包如其名–地形分析
程序接收话题/state_estimation，/registered_scan /joy和/map_clearing
重点在/state_estimation，/registered_scan
state_estimation是小车位姿，registered_scan是map坐标系下的点云
发表话题/terrain_map消息 地形数据
接收为terrainAnalysisExt功能包 作进一步分析
点云回调函数
主要流程为接收点云，按照下图的圆锥滤波点云
这里滤波大多是在map坐标系下计算的，不是sensor坐标系
因此小车不会在坡度不断增加或减少的地方行进，而是map下固定坡度内行进
主函数
1、然后把点云加入到地形图，地形图的维护与loam中的类似
是一个 21x21的1m 二维网格 地图
2、网格地图再取11x11的网格地形图，划分为51x51的0.2m网格
计算每个网格地面高度，默认取最小高度为地面的高度
3、最后把11x11的网格地形图计算与地面的高度差后发布点云
intensity存储 相对区块地面的高度差
4、发布/terrain_map话题，即地形图
#include <math.h>
#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Float32.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
using namespace std;
const double PI = 3.1415926;
double scanVoxelSize = 0.05;
double decayTime = 2.0;
double noDecayDis = 4.0;
double clearingDis = 8.0;
bool clearingCloud = false;
bool useSorting = true;
double quantileZ = 0.25;
bool considerDrop = false;
bool limitGroundLift = false;
double maxGroundLift = 0.15;
bool clearDyObs = false;
double minDyObsDis = 0.3;
double minDyObsAngle = 0;
double minDyObsRelZ = -0.5;
double minDyObsVFOV = -16.0;
double maxDyObsVFOV = 16.0;
int minDyObsPointNum = 1;
bool noDataObstacle = false;
int noDataBlockSkipNum = 0;
int minBlockPointNum = 10;
double vehicleHeight = 1.5;
int voxelPointUpdateThre = 100;
double voxelTimeUpdateThre = 2.0;
double minRelZ = -1.5;
double maxRelZ = 0.2;
double disRatioZ = 0.2;
// terrain voxel parameters
float terrainVoxelSize = 1.0;
int terrainVoxelShiftX = 0;
int terrainVoxelShiftY = 0;
const int terrainVoxelWidth = 21;
int terrainVoxelHalfWidth = (terrainVoxelWidth - 1) / 2;
const int terrainVoxelNum = terrainVoxelWidth * terrainVoxelWidth;
// planar voxel parameters
float planarVoxelSize = 0.2;
const int planarVoxelWidth = 51;
int planarVoxelHalfWidth = (planarVoxelWidth - 1) / 2;
const int planarVoxelNum = planarVoxelWidth * planarVoxelWidth;
pcl::PointCloud<pcl::PointXYZI>::Ptr
laserCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
laserCloudCrop(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
laserCloudDwz(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
terrainCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
terrainCloudElev(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloud[terrainVoxelNum];
int terrainVoxelUpdateNum[terrainVoxelNum] = {0};
float terrainVoxelUpdateTime[terrainVoxelNum] = {0};
float planarVoxelElev[planarVoxelNum] = {0};
int planarVoxelEdge[planarVoxelNum] = {0};
int planarVoxelDyObs[planarVoxelNum] = {0};
vector<float> planarPointElev[planarVoxelNum];
double laserCloudTime = 0;
bool newlaserCloud = false;
double systemInitTime = 0;
bool systemInited = false;
int noDataInited = 0;
float vehicleRoll = 0, vehiclePitch = 0, vehicleYaw = 0;
float vehicleX = 0, vehicleY = 0, vehicleZ = 0;
float vehicleXRec = 0, vehicleYRec = 0;
float sinVehicleRoll = 0, cosVehicleRoll = 0;
float sinVehiclePitch = 0, cosVehiclePitch = 0;
float sinVehicleYaw = 0, cosVehicleYaw = 0;
pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
// state estimation callback function
void odometryHandler(const nav_msgs::Odometry::ConstPtr &odom) {
//提取小车当前 R P Y ；X Y Z
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);
vehicleRoll = roll;
vehiclePitch = pitch;
vehicleYaw = yaw;
vehicleX = odom->pose.pose.position.x;
vehicleY = odom->pose.pose.position.y;
vehicleZ = odom->pose.pose.position.z;
//计算R P Y 的sin和cos
sinVehicleRoll = sin(vehicleRoll);
cosVehicleRoll = cos(vehicleRoll);
sinVehiclePitch = sin(vehiclePitch);
cosVehiclePitch = cos(vehiclePitch);
sinVehicleYaw = sin(vehicleYaw);
cosVehicleYaw = cos(vehicleYaw);
//noDataInited=0 没有小车上一时刻位置
//noDataInited=1 有小车上一时刻位置
//noDataInited=2 有运动距离大于4
if (noDataInited == 0) {
vehicleXRec = vehicleX;
vehicleYRec = vehicleY;
noDataInited = 1;
}
if (noDataInited == 1) {
float dis = sqrt((vehicleX - vehicleXRec) * (vehicleX - vehicleXRec) +
(vehicleY - vehicleYRec) * (vehicleY - vehicleYRec));
if (dis >= noDecayDis)
noDataInited = 2;
}
}
// registered laser scan callback function
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloud2) {
laserCloudTime = laserCloud2->header.stamp.toSec();
//系统初始化
if (!systemInited) {
systemInitTime = laserCloudTime;
systemInited = true;
}
laserCloud->clear();
pcl::fromROSMsg(*laserCloud2, *laserCloud);
//计算每个点
pcl::PointXYZI point;
laserCloudCrop->clear();
int laserCloudSize = laserCloud->points.size();
for (int i = 0; i < laserCloudSize; i++) {
point = laserCloud->points[i];
float pointX = point.x;
float pointY = point.y;
float pointZ = point.z;
//计算每个点 水平到小车的距离
//点云是map坐标系下的
float dis = sqrt((pointX - vehicleX) * (pointX - vehicleX) +
(pointY - vehicleY) * (pointY - vehicleY));
/*
map坐标系下相对小车计算 公式应该是这样的
z > -1.5 - 0.2*sqrt(x^2 + y^2)
z < 0.2 + 0.2*sqrt(x^2 + y^2)
是两个圆锥的夹层
*/
if (pointZ - vehicleZ > minRelZ - disRatioZ * dis &&
pointZ - vehicleZ < maxRelZ + disRatioZ * dis &&
dis < terrainVoxelSize * (terrainVoxelHalfWidth + 1)) {
point.x = pointX;
point.y = pointY;
point.z = pointZ;
point.intensity = laserCloudTime - systemInitTime;
laserCloudCrop->push_back(point);
}
}
newlaserCloud = true;
}
// joystick callback function
void joystickHandler(const sensor_msgs::Joy::ConstPtr &joy) {
if (joy->buttons[5] > 0.5) {
noDataInited = 0;
clearingCloud = true;
}
}
// cloud clearing callback function
void clearingHandler(const std_msgs::Float32::ConstPtr &dis) {
noDataInited = 0;
clearingDis = dis->data;
clearingCloud = true;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "terrainAnalysis");
ros::NodeHandle nh;
ros::NodeHandle nhPrivate = ros::NodeHandle("~");
nhPrivate.getParam("scanVoxelSize", scanVoxelSize);
nhPrivate.getParam("decayTime", decayTime);
nhPrivate.getParam("noDecayDis", noDecayDis);
nhPrivate.getParam("clearingDis", clearingDis);
nhPrivate.getParam("useSorting", useSorting);
nhPrivate.getParam("quantileZ", quantileZ);
nhPrivate.getParam("considerDrop", considerDrop);
nhPrivate.getParam("limitGroundLift", limitGroundLift);
nhPrivate.getParam("maxGroundLift", maxGroundLift);
nhPrivate.getParam("clearDyObs", clearDyObs);
nhPrivate.getParam("minDyObsDis", minDyObsDis);
nhPrivate.getParam("minDyObsAngle", minDyObsAngle);
nhPrivate.getParam("minDyObsRelZ", minDyObsRelZ);
nhPrivate.getParam("minDyObsVFOV", minDyObsVFOV);
nhPrivate.getParam("maxDyObsVFOV", maxDyObsVFOV);
nhPrivate.getParam("minDyObsPointNum", minDyObsPointNum);
nhPrivate.getParam("noDataObstacle", noDataObstacle);
nhPrivate.getParam("noDataBlockSkipNum", noDataBlockSkipNum);
nhPrivate.getParam("minBlockPointNum", minBlockPointNum);
nhPrivate.getParam("vehicleHeight", vehicleHeight);
nhPrivate.getParam("voxelPointUpdateThre", voxelPointUpdateThre);
nhPrivate.getParam("voxelTimeUpdateThre", voxelTimeUpdateThre);
nhPrivate.getParam("minRelZ", minRelZ);
nhPrivate.getParam("maxRelZ", maxRelZ);
nhPrivate.getParam("disRatioZ", disRatioZ);
ros::Subscriber subOdometry =
nh.subscribe<nav_msgs::Odometry>("/state_estimation", 5, odometryHandler);
//map坐标系下的点云
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(
"/registered_scan", 5, laserCloudHandler);
ros::Subscriber subJoystick =
nh.subscribe<sensor_msgs::Joy>("/joy", 5, joystickHandler);
//只启动仿真局部路径规划没用
ros::Subscriber subClearing =
nh.subscribe<std_msgs::Float32>("/map_clearing", 5, clearingHandler);
ros::Publisher pubLaserCloud =
nh.advertise<sensor_msgs::PointCloud2>("/terrain_map", 2);
for (int i = 0; i < terrainVoxelNum; i++) {
terrainVoxelCloud[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
}
downSizeFilter.setLeafSize(scanVoxelSize, scanVoxelSize, scanVoxelSize);
//100Hz循环检测 newlaserCloud
ros::Rate rate(100);
bool status = ros::ok();
while (status) {
ros::spinOnce();
if (newlaserCloud) {
newlaserCloud = false;
// terrain voxel roll over
/*
这一大段都是滑动窗口地图，地图形式与LOAM中的类似
将地图分为terrainVoxelWidth*terrainVoxelWidth 21x21个小地图
小地图 每个宽1x1m 地图总管21x21m
*/
float terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX;
float terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY;
//如果小车 vehicleX 位置小于 terrainVoxelCenX 中心位置一个小地图的宽度 terrainVoxelSize
//就把地图向 X 正方向移动一格
while (vehicleX - terrainVoxelCenX < -terrainVoxelSize) {
for (int indY = 0; indY < terrainVoxelWidth; indY++) {
//X = terrainVoxelWidth - 1即最后一行的点云
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) + indY];
// indY 那一行的点云 indX-1 行移动到 indX 行
for (int indX = terrainVoxelWidth - 1; indX >= 1; indX--) {
terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
terrainVoxelCloud[terrainVoxelWidth * (indX - 1) + indY];
}
//向 X 正方向移动一格的话 indX = 0 那一行的点云就得 clear ，没有 -1 行
//第一行的 X = 0的直接清除
//这里第一行点云用的最后一行点云的指针，指针回收重复利用，降低内存消耗
terrainVoxelCloud[indY] = terrainVoxelCloudPtr;
terrainVoxelCloud[indY]->clear();
}
//新的中心X序号，以及坐标
terrainVoxelShiftX--;
terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX;
}
//同理
while (vehicleX - terrainVoxelCenX > terrainVoxelSize) {
for (int indY = 0; indY < terrainVoxelWidth; indY++) {
//X = 0的第一行点云
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
terrainVoxelCloud[indY];
//X负方向移动一
for (int indX = 0; indX < terrainVoxelWidth - 1; indX++) {
terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
terrainVoxelCloud[terrainVoxelWidth * (indX + 1) + indY];
}
//X = terrainVoxelWidth - 1 即最后一行的点云清空
terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) +
indY] = terrainVoxelCloudPtr;
terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) + indY]
->clear();
}
terrainVoxelShiftX++;
terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX;
}
while (vehicleY - terrainVoxelCenY < -terrainVoxelSize) {
for (int indX = 0; indX < terrainVoxelWidth; indX++) {
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
terrainVoxelCloud[terrainVoxelWidth * indX +
(terrainVoxelWidth - 1)];
for (int indY = terrainVoxelWidth - 1; indY >= 1; indY--) {
terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
terrainVoxelCloud[terrainVoxelWidth * indX + (indY - 1)];
}
terrainVoxelCloud[terrainVoxelWidth * indX] = terrainVoxelCloudPtr;
terrainVoxelCloud[terrainVoxelWidth * indX]->clear();
}
terrainVoxelShiftY--;
terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY;
}
while (vehicleY - terrainVoxelCenY > terrainVoxelSize) {
for (int indX = 0; indX < terrainVoxelWidth; indX++) {
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
terrainVoxelCloud[terrainVoxelWidth * indX];
for (int indY = 0; indY < terrainVoxelWidth - 1; indY++) {
terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
terrainVoxelCloud[terrainVoxelWidth * indX + (indY + 1)];
}
terrainVoxelCloud[terrainVoxelWidth * indX +
(terrainVoxelWidth - 1)] = terrainVoxelCloudPtr;
terrainVoxelCloud[terrainVoxelWidth * indX + (terrainVoxelWidth - 1)]
->clear();
}
terrainVoxelShiftY++;
terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY;
}
// stack registered laser scans
/*
把当前帧的地形点存入地形地图
按照到小车的距离算点云IND，还是map坐标系，以小车为中心
将map坐标系点云存入terrainVoxelCloud
*/
pcl::PointXYZI point;
int laserCloudCropSize = laserCloudCrop->points.size();
for (int i = 0; i < laserCloudCropSize; i++) {
point = laserCloudCrop->points[i];
//计算点云 对应分块地图 坐标indX indY
int indX = int((point.x - vehicleX + terrainVoxelSize / 2) /
terrainVoxelSize) +
terrainVoxelHalfWidth;
int indY = int((point.y - vehicleY + terrainVoxelSize / 2) /
terrainVoxelSize) +
terrainVoxelHalfWidth;
if (point.x - vehicleX + terrainVoxelSize / 2 < 0)
indX--;
if (point.y - vehicleY + terrainVoxelSize / 2 < 0)
indY--;
//加入到 terrainVoxelCloud 对应的点云里
if (indX >= 0 && indX < terrainVoxelWidth && indY >= 0 &&
indY < terrainVoxelWidth) {
terrainVoxelCloud[terrainVoxelWidth * indX + indY]->push_back(point);
terrainVoxelUpdateNum[terrainVoxelWidth * indX + indY]++;
}
}
//这里的含义是 过滤地形地图的点
//虽然点云输入的时候过滤过 但是地图里还有 历史点需要过滤
for (int ind = 0; ind < terrainVoxelNum; ind++) {
//满足以下三个条件中的一个
//加入分区点云点的数量大于 voxelPointUpdateThre
//当前分区点云时间差 大于 该分区距离上次使用的时间差 加上 voxelTimeUpdateThre //很久没更新了这块
//clearingCloud 手动启动
if (terrainVoxelUpdateNum[ind] >= voxelPointUpdateThre ||
laserCloudTime - systemInitTime - terrainVoxelUpdateTime[ind] >= voxelTimeUpdateThre ||
clearingCloud) {
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
terrainVoxelCloud[ind];
laserCloudDwz->clear();
downSizeFilter.setInputCloud(terrainVoxelCloudPtr);
downSizeFilter.filter(*laserCloudDwz);
terrainVoxelCloudPtr->clear();
//以小车为中心取点
//还是那个圆锥形状条件过滤
int laserCloudDwzSize = laserCloudDwz->points.size();
for (int i = 0; i < laserCloudDwzSize; i++) {
point = laserCloudDwz->points[i];
float dis = sqrt((point.x - vehicleX) * (point.x - vehicleX) +
(point.y - vehicleY) * (point.y - vehicleY));
if (point.z - vehicleZ > minRelZ - disRatioZ * dis &&
point.z - vehicleZ < maxRelZ + disRatioZ * dis &&
(laserCloudTime - systemInitTime - point.intensity <
decayTime ||
dis < noDecayDis) &&
!(dis < clearingDis && clearingCloud)) {
terrainVoxelCloudPtr->push_back(point);
}
}
//更新点变 0 更新时间
terrainVoxelUpdateNum[ind] = 0;
terrainVoxelUpdateTime[ind] = laserCloudTime - systemInitTime;
}
}
//取11x11的 网格地图 点作为terrainCloud 地形图
terrainCloud->clear();
for (int indX = terrainVoxelHalfWidth - 5;
indX <= terrainVoxelHalfWidth + 5; indX++) {
for (int indY = terrainVoxelHalfWidth - 5;
indY <= terrainVoxelHalfWidth + 5; indY++) {
*terrainCloud += *terrainVoxelCloud[terrainVoxelWidth * indX + indY];
}
}
// estimate ground and compute elevation for each point
//清空存储信息
for (int i = 0; i < planarVoxelNum; i++) {
planarVoxelElev[i] = 0;
planarVoxelEdge[i] = 0;
planarVoxelDyObs[i] = 0;
planarPointElev[i].clear();
}
int terrainCloudSize = terrainCloud->points.size();
for (int i = 0; i < terrainCloudSize; i++) {
point = terrainCloud->points[i];
//把所有 地形图 terrainCloud 点计算indX和indY后 点的高度Z放入 planarPointElev 水平高度分析网格
//planarPointElev 地面点 每个网格宽0.2m（默认），共51x51 即只考虑10x10m
int indX =
int((point.x - vehicleX + planarVoxelSize / 2) / planarVoxelSize) +
planarVoxelHalfWidth;
int indY =
int((point.y - vehicleY + planarVoxelSize / 2) / planarVoxelSize) +
planarVoxelHalfWidth;
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
indX--;
if (point.y - vehicleY + planarVoxelSize / 2 < 0)
indY--;
if (point.z - vehicleZ > minRelZ && point.z - vehicleZ < maxRelZ) {
for (int dX = -1; dX <= 1; dX++) {
for (int dY = -1; dY <= 1; dY++) {
if (indX + dX >= 0 && indX + dX < planarVoxelWidth &&
indY + dY >= 0 && indY + dY < planarVoxelWidth) {
planarPointElev[planarVoxelWidth * (indX + dX) + indY + dY]
.push_back(point.z);
}
}
}
}
//默认为false 不运行 clearDyObs清代表是否清除障碍物
if (clearDyObs) {
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
indY < planarVoxelWidth) {
//terrainCloud 地形图的点到小车的距离
float pointX1 = point.x - vehicleX;
float pointY1 = point.y - vehicleY;
float pointZ1 = point.z - vehicleZ;
float dis1 = sqrt(pointX1 * pointX1 + pointY1 * pointY1);
if (dis1 > minDyObsDis) {
//水平仰角高（与水平最低0.5比）
float angle1 = atan2(pointZ1 - minDyObsRelZ, dis1) * 180.0 / PI;
if (angle1 > minDyObsAngle) {
//相对小车的坐标，这里是转到了小车坐标系
float pointX2 =
pointX1 * cosVehicleYaw + pointY1 * sinVehicleYaw;
float pointY2 =
-pointX1 * sinVehicleYaw + pointY1 * cosVehicleYaw;
float pointZ2 = pointZ1;
float pointX3 =
pointX2 * cosVehiclePitch - pointZ2 * sinVehiclePitch;
float pointY3 = pointY2;
float pointZ3 =
pointX2 * sinVehiclePitch + pointZ2 * cosVehiclePitch;
float pointX4 = pointX3;
float pointY4 =
pointY3 * cosVehicleRoll + pointZ3 * sinVehicleRoll;
float pointZ4 =
-pointY3 * sinVehicleRoll + pointZ3 * cosVehicleRoll;
//相对小车的水平距离，仰角，在小车坐标系下
float dis4 = sqrt(pointX4 * pointX4 + pointY4 * pointY4);
float angle4 = atan2(pointZ4, dis4) * 180.0 / PI;
//仰角内的 标记为障碍物
if (angle4 > minDyObsVFOV && angle4 < maxDyObsVFOV) {
planarVoxelDyObs[planarVoxelWidth * indX + indY]++;
}
}
} else {
planarVoxelDyObs[planarVoxelWidth * indX + indY] +=
minDyObsPointNum;
}
}
}
}
//默认为false 不运行
if (clearDyObs) {
for (int i = 0; i < laserCloudCropSize; i++) {
point = laserCloudCrop->points[i];
int indX = int((point.x - vehicleX + planarVoxelSize / 2) /
planarVoxelSize) +
planarVoxelHalfWidth;
int indY = int((point.y - vehicleY + planarVoxelSize / 2) /
planarVoxelSize) +
planarVoxelHalfWidth;
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
indX--;
if (point.y - vehicleY + planarVoxelSize / 2 < 0)
indY--;
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
indY < planarVoxelWidth) {
float pointX1 = point.x - vehicleX;
float pointY1 = point.y - vehicleY;
float pointZ1 = point.z - vehicleZ;
float dis1 = sqrt(pointX1 * pointX1 + pointY1 * pointY1);
float angle1 = atan2(pointZ1 - minDyObsRelZ, dis1) * 180.0 / PI;
//清除地图坐标系下的障碍
if (angle1 > minDyObsAngle) {
planarVoxelDyObs[planarVoxelWidth * indX + indY] = 0;
}
}
}
}
//排序，取第1/4低的 的高度作为 planarVoxelElev[i]网格高度
if (useSorting) {
for (int i = 0; i < planarVoxelNum; i++) {
int planarPointElevSize = planarPointElev[i].size();
if (planarPointElevSize > 0) {
sort(planarPointElev[i].begin(), planarPointElev[i].end());
int quantileID = int(quantileZ * planarPointElevSize);
if (quantileID < 0)
quantileID = 0;
else if (quantileID >= planarPointElevSize)
quantileID = planarPointElevSize - 1;
//这里默认limitGroundLift 限制地面高度 为否
//1/4处大于最低点+maxGroundLift 并且 限制地面高度 以1/4处加maxGroundLift为高度
//否则1/4为网格的地面高度
if (planarPointElev[i][quantileID] >
planarPointElev[i][0] + maxGroundLift &&
limitGroundLift) {
planarVoxelElev[i] = planarPointElev[i][0] + maxGroundLift;
} else {
planarVoxelElev[i] = planarPointElev[i][quantileID];
}
}
}
//不排序 以最低点为地面高度
} else {
for (int i = 0; i < planarVoxelNum; i++) {
int planarPointElevSize = planarPointElev[i].size();
if (planarPointElevSize > 0) {
float minZ = 1000.0;
int minID = -1;
for (int j = 0; j < planarPointElevSize; j++) {
if (planarPointElev[i][j] < minZ) {
minZ = planarPointElev[i][j];
minID = j;
}
}
if (minID != -1) {
planarVoxelElev[i] = planarPointElev[i][minID];
}
}
}
}
terrainCloudElev->clear();
int terrainCloudElevSize = 0;
//取地形点云的点 11x11
for (int i = 0; i < terrainCloudSize; i++) {
point = terrainCloud->points[i];
if (point.z - vehicleZ > minRelZ && point.z - vehicleZ < maxRelZ) {
//计算在 所在地面网格坐标
int indX = int((point.x - vehicleX + planarVoxelSize / 2) /
planarVoxelSize) +
planarVoxelHalfWidth;
int indY = int((point.y - vehicleY + planarVoxelSize / 2) /
planarVoxelSize) +
planarVoxelHalfWidth;
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
indX--;
if (point.y - vehicleY + planarVoxelSize / 2 < 0)
indY--;
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
indY < planarVoxelWidth) {
//没有障碍物 或 清除过障碍物
if (planarVoxelDyObs[planarVoxelWidth * indX + indY] <
minDyObsPointNum ||
!clearDyObs) {
//相对网格地面的高度
float disZ =
point.z - planarVoxelElev[planarVoxelWidth * indX + indY];
if (considerDrop)
disZ = fabs(disZ);
int planarPointElevSize =
planarPointElev[planarVoxelWidth * indX + indY].size();
//当前点 相对网格地面的 Z轴，大于地面，小于小车高度 并且 网格点的数量大于 minBlockPointNum
//把点放入 terrainCloudElev 地形图中
//intensity存储 相对网格地面的高度
if (disZ >= 0 && disZ < vehicleHeight &&
planarPointElevSize >= minBlockPointNum) {
terrainCloudElev->push_back(point);
terrainCloudElev->points[terrainCloudElevSize].intensity = disZ;
terrainCloudElevSize++;
}
}
}
}
}
//noDataObstacle 默认为false 不运行
//noDataInited == 2 代表 当小车上一时刻位置与这一时刻距离较远
if (noDataObstacle && noDataInited == 2) {
for (int i = 0; i < planarVoxelNum; i++) {
int planarPointElevSize = planarPointElev[i].size();
if (planarPointElevSize < minBlockPointNum) {
planarVoxelEdge[i] = 1;
}
}
for (int noDataBlockSkipCount = 0;
noDataBlockSkipCount < noDataBlockSkipNum;
noDataBlockSkipCount++) {
for (int i = 0; i < planarVoxelNum; i++) {
if (planarVoxelEdge[i] >= 1) {
int indX = int(i / planarVoxelWidth);
int indY = i % planarVoxelWidth;
bool edgeVoxel = false;
for (int dX = -1; dX <= 1; dX++) {
for (int dY = -1; dY <= 1; dY++) {
if (indX + dX >= 0 && indX + dX < planarVoxelWidth &&
indY + dY >= 0 && indY + dY < planarVoxelWidth) {
if (planarVoxelEdge[planarVoxelWidth * (indX + dX) + indY +
dY] < planarVoxelEdge[i]) {
edgeVoxel = true;
}
}
}
}
if (!edgeVoxel)
planarVoxelEdge[i]++;
}
}
}
for (int i = 0; i < planarVoxelNum; i++) {
if (planarVoxelEdge[i] > noDataBlockSkipNum) {
int indX = int(i / planarVoxelWidth);
int indY = i % planarVoxelWidth;
point.x =
planarVoxelSize * (indX - planarVoxelHalfWidth) + vehicleX;
point.y =
planarVoxelSize * (indY - planarVoxelHalfWidth) + vehicleY;
point.z = vehicleZ;
point.intensity = vehicleHeight;
point.x -= planarVoxelSize / 4.0;
point.y -= planarVoxelSize / 4.0;
terrainCloudElev->push_back(point);
point.x += planarVoxelSize / 2.0;
terrainCloudElev->push_back(point);
point.y += planarVoxelSize / 2.0;
terrainCloudElev->push_back(point);
point.x -= planarVoxelSize / 2.0;
terrainCloudElev->push_back(point);
}
}
}
clearingCloud = false;
// publish points with elevation
sensor_msgs::PointCloud2 terrainCloud2;
pcl::toROSMsg(*terrainCloudElev, terrainCloud2);
terrainCloud2.header.stamp = ros::Time().fromSec(laserCloudTime);
terrainCloud2.header.frame_id = "map";
pubLaserCloud.publish(terrainCloud2);
}
status = ros::ok();
rate.sleep();
}
return 0;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
- 210
- 211
- 212
- 213
- 214
- 215
- 216
- 217
- 218
- 219
- 220
- 221
- 222
- 223
- 224
- 225
- 226
- 227
- 228
- 229
- 230
- 231
- 232
- 233
- 234
- 235
- 236
- 237
- 238
- 239
- 240
- 241
- 242
- 243
- 244
- 245
- 246
- 247
- 248
- 249
- 250
- 251
- 252
- 253
- 254
- 255
- 256
- 257
- 258
- 259
- 260
- 261
- 262
- 263
- 264
- 265
- 266
- 267
- 268
- 269
- 270
- 271
- 272
- 273
- 274
- 275
- 276
- 277
- 278
- 279
- 280
- 281
- 282
- 283
- 284
- 285
- 286
- 287
- 288
- 289
- 290
- 291
- 292
- 293
- 294
- 295
- 296
- 297
- 298
- 299
- 300
- 301
- 302
- 303
- 304
- 305
- 306
- 307
- 308
- 309
- 310
- 311
- 312
- 313
- 314
- 315
- 316
- 317
- 318
- 319
- 320
- 321
- 322
- 323
- 324
- 325
- 326
- 327
- 328
- 329
- 330
- 331
- 332
- 333
- 334
- 335
- 336
- 337
- 338
- 339
- 340
- 341
- 342
- 343
- 344
- 345
- 346
- 347
- 348
- 349
- 350
- 351
- 352
- 353
- 354
- 355
- 356
- 357
- 358
- 359
- 360
- 361
- 362
- 363
- 364
- 365
- 366
- 367
- 368
- 369
- 370
- 371
- 372
- 373
- 374
- 375
- 376
- 377
- 378
- 379
- 380
- 381
- 382
- 383
- 384
- 385
- 386
- 387
- 388
- 389
- 390
- 391
- 392
- 393
- 394
- 395
- 396
- 397
- 398
- 399
- 400
- 401
- 402
- 403
- 404
- 405
- 406
- 407
- 408
- 409
- 410
- 411
- 412
- 413
- 414
- 415
- 416
- 417
- 418
- 419
- 420
- 421
- 422
- 423
- 424
- 425
- 426
- 427
- 428
- 429
- 430
- 431
- 432
- 433
- 434
- 435
- 436
- 437
- 438
- 439
- 440
- 441
- 442
- 443
- 444
- 445
- 446
- 447
- 448
- 449
- 450
- 451
- 452
- 453
- 454
- 455
- 456
- 457
- 458
- 459
- 460
- 461
- 462
- 463
- 464
- 465
- 466
- 467
- 468
- 469
- 470
- 471
- 472
- 473
- 474
- 475
- 476
- 477
- 478
- 479
- 480
- 481
- 482
- 483
- 484
- 485
- 486
- 487
- 488
- 489
- 490
- 491
- 492
- 493
- 494
- 495
- 496
- 497
- 498
- 499
- 500
- 501
- 502
- 503
- 504
- 505
- 506
- 507
- 508
- 509
- 510
- 511
- 512
- 513
- 514
- 515
- 516
- 517
- 518
- 519
- 520
- 521
- 522
- 523
- 524
- 525
- 526
- 527
- 528
- 529
- 530
- 531
- 532
- 533
- 534
- 535
- 536
- 537
- 538
- 539
- 540
- 541
- 542
- 543
- 544
- 545
- 546
- 547
- 548
- 549
- 550
- 551
- 552
- 553
- 554
- 555
- 556
- 557
- 558
- 559
- 560
- 561
- 562
- 563
- 564
- 565
- 566
- 567
- 568
- 569
- 570
- 571
- 572
- 573
- 574
- 575
- 576
- 577
- 578
- 579
- 580
- 581
- 582
- 583
- 584
- 585
- 586
- 587
- 588
- 589
- 590
- 591
- 592
- 593
- 594
- 595
- 596
- 597
- 598
- 599
- 600
- 601
- 602
- 603
- 604
- 605
- 606
- 607
- 608
- 609
- 610
- 611
- 612
- 613
- 614
- 615
- 616
- 617
- 618
- 619
- 620
- 621
- 622
- 623
- 624
- 625
- 626
- 627
- 628
- 629
- 630
- 631
- 632
- 633
- 634
- 635
- 636
- 637
- 638
- 639
- 640
- 641
- 642
- 643
- 644
- 645
- 646
- 647
- 648
- 649
- 650
- 651
- 652
- 653
- 654
- 655
- 656
- 657
- 658
- 659
- 660
- 661
- 662
- 663
- 664
- 665
- 666
- 667
- 668
- 669
- 670
- 671
- 672
- 673
- 674
- 675
- 676
- 677
- 678
- 679
- 680
- 681
- 682
- 683
- 684
- 685
- 686
- 687
- 688
- 689
- 690
- 691
- 692
- 693
- 694
- 695
- 696
- 697
- 698
- 699
- 700
- 701
- 702
- 703
- 704
- 705
- 706
- 707
- 708
- 709
- 710
- 711
- 712
- 713
- 714
- 715
- 716
- 717
- 718
- 719
- 720
- 721
- 722
- 723
- 724
- 725
- 726
- 727
- 728
- 729
- 730
- 731
- 732
- 733
- 734
- 735
- 736
- 737
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/菜鸟追梦旅行/article/detail/371781?site
推荐阅读
相关标签
