额外开启一个线程检测并更新地图
void* thread_func(void* args)
{
ros::NodeHandle nh_map;
ros::CallbackQueue map_callback_queue;
nh_map.setCallbackQueue(&map_callback_queue);
ros::Subscriber map_sub = nh_map.subscribe("points_map", 10, map_callback);
ros::Rate ros_rate(10);
while (nh_map.ok())
{
map_callback_queue.callAvailable(ros::WallDuration());
ros_rate.sleep();
}“map_callback” 地图回调函数
static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
// if (map_loaded == 0)
if (points_map_num != input->width) //points_map_num默认为0，如果和收到的地图数据大小不一致代表地图更新了
{
std::cout << "Update points_map." << std::endl;
points_map_num = input->width;
// Convert the data type(from sensor_msgs to pcl).
pcl::fromROSMsg(*input, map);
if (_use_local_transform == true)//默认为false，不知道是干嘛的
{
tf::TransformListener local_transform_listener;
try
{
ros::Time now = ros::Time(0);
local_transform_listener.waitForTransform("/map", "/world", now, ros::Duration(10.0));
local_transform_listener.lookupTransform("/map", "world", now, local_transform);
}
catch (tf::TransformException& ex)
{
ROS_ERROR("%s", ex.what());
}
pcl_ros::transformPointCloud(map, map, local_transform.inverse());
}
pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(map));
// Setting point cloud to be aligned to.
/*
MethodType::PCL_GENERIC == 0
MethodType::PCL_ANH == 1
MethodType::PCL_ANH_GPU == 2
MethodType::PCL_OPENMP == 3
*/
if (_method_type == MethodType::PCL_GENERIC) //_method_type默认为0
{
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_ndt;//创建了带默认参数的NDT算法对象。
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
new_ndt.setResolution(ndt_res); //设置NDT网格网格结构的分辨率（voxelgridcovariance）
new_ndt.setInputTarget(map_ptr); //目标点云
new_ndt.setMaximumIterations(max_iter); //这个参数控制了优化过程的最大迭代次数。一般来说，在达到最大迭代次数之前程序就会先达到epsilon阈值而终止。添加最大迭代次数的限制能够增加程序鲁棒性，阻止了它在错误的方向运行过长时间
new_ndt.setStepSize(step_size); //为more-thuente线搜索设置最大步长
new_ndt.setTransformationEpsilon(trans_eps); //为终止条件设置最小转换差异
new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());//点云配准，变换后的点云保存在output cloud里，之后打印出配准分数。分数通过计算output cloud与target cloud对应的最近点欧式距离的平方和得到，得分越小说明匹配效果越好。
pthread_mutex_lock(&mutex);
ndt = new_ndt;
pthread_mutex_unlock(&mutex);
}
else if (_method_type == MethodType::PCL_ANH)
{
cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_anh_ndt;
new_anh_ndt.setResolution(ndt_res);
new_anh_ndt.setInputTarget(map_ptr);
new_anh_ndt.setMaximumIterations(max_iter);
new_anh_ndt.setStepSize(step_size);
new_anh_ndt.setTransformationEpsilon(trans_eps);
pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointXYZ dummy_point;
dummy_scan_ptr->push_back(dummy_point);
new_anh_ndt.setInputSource(dummy_scan_ptr);
new_anh_ndt.align(Eigen::Matrix4f::Identity());
pthread_mutex_lock(&mutex);
anh_ndt = new_anh_ndt;
pthread_mutex_unlock(&mutex);
}
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
{
std::shared_ptr<gpu::GNormalDistributionsTransform> new_anh_gpu_ndt_ptr =
std::make_shared<gpu::GNormalDistributionsTransform>();
new_anh_gpu_ndt_ptr->setResolution(ndt_res);
new_anh_gpu_ndt_ptr->setInputTarget(map_ptr);
new_anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
new_anh_gpu_ndt_ptr->setStepSize(step_size);
new_anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointXYZ dummy_point;
dummy_scan_ptr->push_back(dummy_point);
new_anh_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);
new_anh_gpu_ndt_ptr->align(Eigen::Matrix4f::Identity());
pthread_mutex_lock(&mutex);
anh_gpu_ndt_ptr = new_anh_gpu_ndt_ptr;
pthread_mutex_unlock(&mutex);
}
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
{
pcl_omp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_omp_ndt;
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
new_omp_ndt.setResolution(ndt_res);
new_omp_ndt.setInputTarget(map_ptr);
new_omp_ndt.setMaximumIterations(max_iter);
new_omp_ndt.setStepSize(step_size);
new_omp_ndt.setTransformationEpsilon(trans_eps);
new_omp_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
pthread_mutex_lock(&mutex);
omp_ndt = new_omp_ndt;
pthread_mutex_unlock(&mutex);
}
#endif
map_loaded = 1;
}
}“param_callback” 默认参数初始化配置
static void param_callback(const autoware_config_msgs::ConfigNDT::ConstPtr& input)
{
//传入的参数是否与默认参数相同，不同则重置标志位
if (_use_gnss != input->init_pos_gnss)
{
init_pos_set = 0;
}
else if (_use_gnss == 0 &&
(initial_pose.x != input->x || initial_pose.y != input->y || initial_pose.z != input->z ||
initial_pose.roll != input->roll || initial_pose.pitch != input->pitch || initial_pose.yaw != input->yaw))
{
init_pos_set = 0;
}
_use_gnss = input->init_pos_gnss;
// Setting parameters
//判断传入的参数和默认的参数是否相同，不同则改为传入的参数
if (input->resolution != ndt_res)
{
ndt_res = input->resolution;
if (_method_type == MethodType::PCL_GENERIC)
ndt.setResolution(ndt_res);
else if (_method_type == MethodType::PCL_ANH)
anh_ndt.setResolution(ndt_res);
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
anh_gpu_ndt_ptr->setResolution(ndt_res);
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
omp_ndt.setResolution(ndt_res);
#endif
}
if (input->step_size != step_size)
{
step_size = input->step_size;
if (_method_type == MethodType::PCL_GENERIC)
ndt.setStepSize(step_size);
else if (_method_type == MethodType::PCL_ANH)
anh_ndt.setStepSize(step_size);
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
anh_gpu_ndt_ptr->setStepSize(step_size);
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
omp_ndt.setStepSize(ndt_res);
#endif
}
if (input->trans_epsilon != trans_eps)
{
trans_eps = input->trans_epsilon;
if (_method_type == MethodType::PCL_GENERIC)
ndt.setTransformationEpsilon(trans_eps);
else if (_method_type == MethodType::PCL_ANH)
anh_ndt.setTransformationEpsilon(trans_eps);
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
anh_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
omp_ndt.setTransformationEpsilon(ndt_res);
#endif
}
if (input->max_iterations != max_iter)
{
max_iter = input->max_iterations;
if (_method_type == MethodType::PCL_GENERIC)
ndt.setMaximumIterations(max_iter);
else if (_method_type == MethodType::PCL_ANH)
anh_ndt.setMaximumIterations(max_iter);
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
anh_gpu_ndt_ptr->setMaximumIterations(max_iter);
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
omp_ndt.setMaximumIterations(ndt_res);
#endif
}
//如果不开启gnss且初始化标志位为0
if (_use_gnss == 0 && init_pos_set == 0)
{
initial_pose.x = input->x;
initial_pose.y = input->y;
initial_pose.z = input->z;
initial_pose.roll = input->roll;
initial_pose.pitch = input->pitch;
initial_pose.yaw = input->yaw;
if (_use_local_transform == true)
{
tf::Vector3 v(input->x, input->y, input->z);
tf::Quaternion q;
q.setRPY(input->roll, input->pitch, input->yaw);
tf::Transform transform(q, v);
initial_pose.x = (local_transform.inverse() * transform).getOrigin().getX();
initial_pose.y = (local_transform.inverse() * transform).getOrigin().getY();
initial_pose.z = (local_transform.inverse() * transform).getOrigin().getZ();
tf::Matrix3x3 m(q);
m.getRPY(initial_pose.roll, initial_pose.pitch, initial_pose.yaw);
std::cout << "initial_pose.x: " << initial_pose.x << std::endl;
std::cout << "initial_pose.y: " << initial_pose.y << std::endl;
std::cout << "initial_pose.z: " << initial_pose.z << std::endl;
std::cout << "initial_pose.roll: " << initial_pose.roll << std::endl;
std::cout << "initial_pose.pitch: " << initial_pose.pitch << std::endl;
std::cout << "initial_pose.yaw: " << initial_pose.yaw << std::endl;
}
// Setting position and posture for the first time.
/*把传入的参数设置为初始化参数
localizer_pose 定位器
previous_pose 以前的数据
current_pose 当前的数据
*/
localizer_pose.x = initial_pose.x;
localizer_pose.y = initial_pose.y;
localizer_pose.z = initial_pose.z;
localizer_pose.roll = initial_pose.roll;
localizer_pose.pitch = initial_pose.pitch;
localizer_pose.yaw = initial_pose.yaw;
previous_pose.x = initial_pose.x;
previous_pose.y = initial_pose.y;
previous_pose.z = initial_pose.z;
previous_pose.roll = initial_pose.roll;
previous_pose.pitch = initial_pose.pitch;
previous_pose.yaw = initial_pose.yaw;
current_pose.x = initial_pose.x;
current_pose.y = initial_pose.y;
current_pose.z = initial_pose.z;
current_pose.roll = initial_pose.roll;
current_pose.pitch = initial_pose.pitch;
current_pose.yaw = initial_pose.yaw;
current_velocity = 0;
current_velocity_x = 0;
current_velocity_y = 0;
current_velocity_z = 0;
angular_velocity = 0;
current_pose_imu.x = 0;
current_pose_imu.y = 0;
current_pose_imu.z = 0;
current_pose_imu.roll = 0;
current_pose_imu.pitch = 0;
current_pose_imu.yaw = 0;
current_velocity_imu_x = current_velocity_x;
current_velocity_imu_y = current_velocity_y;
current_velocity_imu_z = current_velocity_z;
printf("genggai--------------1");
init_pos_set = 1;
}
}"points_callback" ndt匹配定位模块
static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
if (map_loaded == 1 && init_pos_set == 1) //map_loaded和init_pos_set默认都为0,收到地图后map_loaded为1,参数初始化或者设置位置之后init_pos_set为1
{
matching_start = std::chrono::system_clock::now();//获取当前系统时间
static tf::TransformBroadcaster br;//tf广播
tf::Transform transform;
/*
predict 速度积分预估
previous 上一时刻
ndt 正态分布匹配出来的base_link位置
current 当前时刻最终定位
localizer 正态分布匹配出来的lidar位置
*/
tf::Quaternion predict_q, ndt_q, current_q, localizer_q;
pcl::PointXYZ p;
pcl::PointCloud<pcl::PointXYZ> filtered_scan;
ros::Time current_scan_time = input->header.stamp;
static ros::Time previous_scan_time = current_scan_time;
pcl::fromROSMsg(*input, filtered_scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(filtered_scan));
int scan_points_num = filtered_scan_ptr->size();
//初始化矩阵
Eigen::Matrix4f t(Eigen::Matrix4f::Identity()); // base_link
Eigen::Matrix4f t2(Eigen::Matrix4f::Identity()); // localizer
std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start,
getFitnessScore_end;
static double align_time, getFitnessScore_time = 0.0;
pthread_mutex_lock(&mutex);
//加载配准点云
if (_method_type == MethodType::PCL_GENERIC)
ndt.setInputSource(filtered_scan_ptr);
else if (_method_type == MethodType::PCL_ANH)
anh_ndt.setInputSource(filtered_scan_ptr);
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
anh_gpu_ndt_ptr->setInputSource(filtered_scan_ptr);
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
omp_ndt.setInputSource(filtered_scan_ptr);
#endif
// Guess the initial gross estimation of the transformation
// 当前帧的雷达时间和上一帧的雷达时间间隔
double diff_time = (current_scan_time - previous_scan_time).toSec();
//速度积分，上一时刻速度*间隔时间
if (_offset == "linear")
{
offset_x = current_velocity_x * diff_time;
offset_y = current_velocity_y * diff_time;
offset_z = current_velocity_z * diff_time;
offset_yaw = angular_velocity * diff_time;
}
else if (_offset == "quadratic")
{
offset_x = (current_velocity_x + current_accel_x * diff_time) * diff_time;
offset_y = (current_velocity_y + current_accel_y * diff_time) * diff_time;
offset_z = current_velocity_z * diff_time;
offset_yaw = angular_velocity * diff_time;
}
else if (_offset == "zero")
{
offset_x = 0.0;
offset_y = 0.0;
offset_z = 0.0;
offset_yaw = 0.0;
}
predict_pose.x = previous_pose.x + offset_x;
predict_pose.y = previous_pose.y + offset_y;
predict_pose.z = previous_pose.z + offset_z;
predict_pose.roll = previous_pose.roll;
predict_pose.pitch = previous_pose.pitch;
predict_pose.yaw = previous_pose.yaw + offset_yaw;
if (_use_imu == true && _use_odom == true)
imu_odom_calc(current_scan_time);
if (_use_imu == true && _use_odom == false)
imu_calc(current_scan_time);
if (_use_imu == false && _use_odom == true)
odom_calc(current_scan_time);
pose predict_pose_for_ndt;
if (_use_imu == true && _use_odom == true)
predict_pose_for_ndt = predict_pose_imu_odom;
else if (_use_imu == true && _use_odom == false)
predict_pose_for_ndt = predict_pose_imu;
else if (_use_imu == false && _use_odom == true)
predict_pose_for_ndt = predict_pose_odom;
else
predict_pose_for_ndt = predict_pose;//最终选择
//提供了点云配准变换，由之前的(x,y,z,roll,pitch,yaw)求出旋转矩阵
Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (_method_type == MethodType::PCL_GENERIC)//默认_method_type为0
{
align_start = std::chrono::system_clock::now();
ndt.align(*output_cloud, init_guess);//进行配准打分和输出
align_end = std::chrono::system_clock::now();
has_converged = ndt.hasConverged();//正态分布收敛值
t = ndt.getFinalTransformation();//得出的变换矩阵
iteration = ndt.getFinalNumIteration();//获取计算对齐所需的迭代次数
getFitnessScore_start = std::chrono::system_clock::now();
fitness_score = ndt.getFitnessScore();//匹配分数
getFitnessScore_end = std::chrono::system_clock::now();
trans_probability = ndt.getTransformationProbability();//转化概率
}
else if (_method_type == MethodType::PCL_ANH)
{
align_start = std::chrono::system_clock::now();
anh_ndt.align(init_guess);
align_end = std::chrono::system_clock::now();
has_converged = anh_ndt.hasConverged();
t = anh_ndt.getFinalTransformation();
iteration = anh_ndt.getFinalNumIteration();
getFitnessScore_start = std::chrono::system_clock::now();
fitness_score = anh_ndt.getFitnessScore();
getFitnessScore_end = std::chrono::system_clock::now();
trans_probability = anh_ndt.getTransformationProbability();
}
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
{
align_start = std::chrono::system_clock::now();
anh_gpu_ndt_ptr->align(init_guess);
align_end = std::chrono::system_clock::now();
has_converged = anh_gpu_ndt_ptr->hasConverged();
t = anh_gpu_ndt_ptr->getFinalTransformation();
iteration = anh_gpu_ndt_ptr->getFinalNumIteration();
getFitnessScore_start = std::chrono::system_clock::now();
fitness_score = anh_gpu_ndt_ptr->getFitnessScore();
getFitnessScore_end = std::chrono::system_clock::now();
trans_probability = anh_gpu_ndt_ptr->getTransformationProbability();
}
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
{
align_start = std::chrono::system_clock::now();
omp_ndt.align(*output_cloud, init_guess);
align_end = std::chrono::system_clock::now();
has_converged = omp_ndt.hasConverged();
t = omp_ndt.getFinalTransformation();
iteration = omp_ndt.getFinalNumIteration();
getFitnessScore_start = std::chrono::system_clock::now();
fitness_score = omp_ndt.getFitnessScore();
getFitnessScore_end = std::chrono::system_clock::now();
trans_probability = omp_ndt.getTransformationProbability();
}
#endif
//配准打分的时间
align_time = std::chrono::duration_cast<std::chrono::microseconds>(align_end - align_start).count() / 1000.0;
t2 = t * tf_btol.inverse();//tf_btol.inverse()：机器人到雷达的静态变换矩阵 t：雷达到点云的变换矩阵
//获取匹配分数的时间
getFitnessScore_time =
std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() /
1000.0;
pthread_mutex_unlock(&mutex);
tf::Matrix3x3 mat_l; // localizer
mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
// Update localizer_pose
localizer_pose.x = t(0, 3);
localizer_pose.y = t(1, 3);
localizer_pose.z = t(2, 3);
mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
tf::Matrix3x3 mat_b; // base_link
mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));
// Update ndt_pose
ndt_pose.x = t2(0, 3);
ndt_pose.y = t2(1, 3);
ndt_pose.z = t2(2, 3);
mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
// Calculate the difference between ndt_pose and predict_pose
// 计算ndt_pose和predict_pose之间的差异
//predict_pose_for_ndt为这次位置的速度积分值
predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
(ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
(ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
if (predict_pose_error <= PREDICT_POSE_THRESHOLD)//PREDICT_POSE_THRESHOLD默认值为0.5
{
use_predict_pose = 0;
}
else
{
use_predict_pose = 1;
}
use_predict_pose = 0;
if (use_predict_pose == 0)//如果为0，说明速度积分值和计算匹配值相近，则说明定位没有跳跃，设当前位置为计算匹配值
{
current_pose.x = ndt_pose.x;
current_pose.y = ndt_pose.y;
current_pose.z = ndt_pose.z;
current_pose.roll = ndt_pose.roll;
current_pose.pitch = ndt_pose.pitch;
current_pose.yaw = ndt_pose.yaw;
}
else//如果为1，说明预估值和计算匹配值相差教导，则说明定位出现了跳跃，设当前位置为速度积分值
{
current_pose.x = predict_pose_for_ndt.x;
current_pose.y = predict_pose_for_ndt.y;
current_pose.z = predict_pose_for_ndt.z;
current_pose.roll = predict_pose_for_ndt.roll;
current_pose.pitch = predict_pose_for_ndt.pitch;
current_pose.yaw = predict_pose_for_ndt.yaw;
}
// Compute the velocity and acceleration
//计算速度和加速度
//位置差异 = 当前位置 - 上一位置
diff_x = current_pose.x - previous_pose.x;
diff_y = current_pose.y - previous_pose.y;
diff_z = current_pose.z - previous_pose.z;
diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
//当前速度 = 位置差异/时间间隔
current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;
current_pose_imu.x = current_pose.x;
current_pose_imu.y = current_pose.y;
current_pose_imu.z = current_pose.z;
current_pose_imu.roll = current_pose.roll;
current_pose_imu.pitch = current_pose.pitch;
current_pose_imu.yaw = current_pose.yaw;
current_velocity_imu_x = current_velocity_x;
current_velocity_imu_y = current_velocity_y;
current_velocity_imu_z = current_velocity_z;
current_pose_odom.x = current_pose.x;
current_pose_odom.y = current_pose.y;
current_pose_odom.z = current_pose.z;
current_pose_odom.roll = current_pose.roll;
current_pose_odom.pitch = current_pose.pitch;
current_pose_odom.yaw = current_pose.yaw;
current_pose_imu_odom.x = current_pose.x;
current_pose_imu_odom.y = current_pose.y;
current_pose_imu_odom.z = current_pose.z;
current_pose_imu_odom.roll = current_pose.roll;
current_pose_imu_odom.pitch = current_pose.pitch;
current_pose_imu_odom.yaw = current_pose.yaw;
//当前速度平滑值 = （当前速度+上一时间速度+上上一时间速度）/3.0
current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
if (current_velocity_smooth < 0.2)
{
current_velocity_smooth = 0.0;
}
//当前加速度 = （当前速度 - 上一时刻速度）/时间间隔
current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;
//估计速度m/s,km/h
estimated_vel_mps.data = current_velocity;
estimated_vel_kmph.data = current_velocity * 3.6;
estimated_vel_mps_pub.publish(estimated_vel_mps);
estimated_vel_kmph_pub.publish(estimated_vel_kmph);
// Set values for publishing pose
//速度积分预估值的姿态
predict_q.setRPY(predict_pose.roll, predict_pose.pitch, predict_pose.yaw);
if (_use_local_transform == true)
{
tf::Vector3 v(predict_pose.x, predict_pose.y, predict_pose.z);
tf::Transform transform(predict_q, v);
predict_pose_msg.header.frame_id = "/map";
predict_pose_msg.header.stamp = current_scan_time;
predict_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
predict_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
predict_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
predict_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
predict_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
predict_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
predict_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
}
else //默认为false
{
predict_pose_msg.header.frame_id = "/map";
predict_pose_msg.header.stamp = current_scan_time;
predict_pose_msg.pose.position.x = predict_pose.x;
predict_pose_msg.pose.position.y = predict_pose.y;
predict_pose_msg.pose.position.z = predict_pose.z;
predict_pose_msg.pose.orientation.x = predict_q.x();
predict_pose_msg.pose.orientation.y = predict_q.y();
predict_pose_msg.pose.orientation.z = predict_q.z();
predict_pose_msg.pose.orientation.w = predict_q.w();
}
tf::Quaternion predict_q_imu;
predict_q_imu.setRPY(predict_pose_imu.roll, predict_pose_imu.pitch, predict_pose_imu.yaw);
predict_pose_imu_msg.header.frame_id = "map";
predict_pose_imu_msg.header.stamp = input->header.stamp;
predict_pose_imu_msg.pose.position.x = predict_pose_imu.x;
predict_pose_imu_msg.pose.position.y = predict_pose_imu.y;
predict_pose_imu_msg.pose.position.z = predict_pose_imu.z;
predict_pose_imu_msg.pose.orientation.x = predict_q_imu.x();
predict_pose_imu_msg.pose.orientation.y = predict_q_imu.y();
predict_pose_imu_msg.pose.orientation.z = predict_q_imu.z();
predict_pose_imu_msg.pose.orientation.w = predict_q_imu.w();
predict_pose_imu_pub.publish(predict_pose_imu_msg);
tf::Quaternion predict_q_odom;
predict_q_odom.setRPY(predict_pose_odom.roll, predict_pose_odom.pitch, predict_pose_odom.yaw);
predict_pose_odom_msg.header.frame_id = "map";
predict_pose_odom_msg.header.stamp = input->header.stamp;
predict_pose_odom_msg.pose.position.x = predict_pose_odom.x;
predict_pose_odom_msg.pose.position.y = predict_pose_odom.y;
predict_pose_odom_msg.pose.position.z = predict_pose_odom.z;
predict_pose_odom_msg.pose.orientation.x = predict_q_odom.x();
predict_pose_odom_msg.pose.orientation.y = predict_q_odom.y();
predict_pose_odom_msg.pose.orientation.z = predict_q_odom.z();
predict_pose_odom_msg.pose.orientation.w = predict_q_odom.w();
predict_pose_odom_pub.publish(predict_pose_odom_msg);
tf::Quaternion predict_q_imu_odom;
predict_q_imu_odom.setRPY(predict_pose_imu_odom.roll, predict_pose_imu_odom.pitch, predict_pose_imu_odom.yaw);
predict_pose_imu_odom_msg.header.frame_id = "map";
predict_pose_imu_odom_msg.header.stamp = input->header.stamp;
predict_pose_imu_odom_msg.pose.position.x = predict_pose_imu_odom.x;
predict_pose_imu_odom_msg.pose.position.y = predict_pose_imu_odom.y;
predict_pose_imu_odom_msg.pose.position.z = predict_pose_imu_odom.z;
predict_pose_imu_odom_msg.pose.orientation.x = predict_q_imu_odom.x();
predict_pose_imu_odom_msg.pose.orientation.y = predict_q_imu_odom.y();
predict_pose_imu_odom_msg.pose.orientation.z = predict_q_imu_odom.z();
predict_pose_imu_odom_msg.pose.orientation.w = predict_q_imu_odom.w();
predict_pose_imu_odom_pub.publish(predict_pose_imu_odom_msg);
//ndt_pose 计算出来的当前位置信息
ndt_q.setRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw);
if (_use_local_transform == true)
{
tf::Vector3 v(ndt_pose.x, ndt_pose.y, ndt_pose.z);
tf::Transform transform(ndt_q, v);
ndt_pose_msg.header.frame_id = "/map";
ndt_pose_msg.header.stamp = current_scan_time;
ndt_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
ndt_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
ndt_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
ndt_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
ndt_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
ndt_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
ndt_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
}
else //默认值
{
ndt_pose_msg.header.frame_id = "/map";
ndt_pose_msg.header.stamp = current_scan_time;
ndt_pose_msg.pose.position.x = ndt_pose.x;
ndt_pose_msg.pose.position.y = ndt_pose.y;
ndt_pose_msg.pose.position.z = ndt_pose.z;
ndt_pose_msg.pose.orientation.x = ndt_q.x();
ndt_pose_msg.pose.orientation.y = ndt_q.y();
ndt_pose_msg.pose.orientation.z = ndt_q.z();
ndt_pose_msg.pose.orientation.w = ndt_q.w();
}
//current_pose 当前判断出来的位置，排除了异常匹配的更精准的位置
current_q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
// current_pose is published by vel_pose_mux
/*
current_pose_msg.header.frame_id = "/map";
current_pose_msg.header.stamp = current_scan_time;
current_pose_msg.pose.position.x = current_pose.x;
current_pose_msg.pose.position.y = current_pose.y;
current_pose_msg.pose.position.z = current_pose.z;
current_pose_msg.pose.orientation.x = current_q.x();
current_pose_msg.pose.orientation.y = current_q.y();
current_pose_msg.pose.orientation.z = current_q.z();
current_pose_msg.pose.orientation.w = current_q.w();
*/
//localizer_pose 当前雷达的位置
localizer_q.setRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw);
if (_use_local_transform == true)
{
tf::Vector3 v(localizer_pose.x, localizer_pose.y, localizer_pose.z);
tf::Transform transform(localizer_q, v);
localizer_pose_msg.header.frame_id = "/map";
localizer_pose_msg.header.stamp = current_scan_time;
localizer_pose_msg.pose.position.x = (local_transform * transform).getOrigin().getX();
localizer_pose_msg.pose.position.y = (local_transform * transform).getOrigin().getY();
localizer_pose_msg.pose.position.z = (local_transform * transform).getOrigin().getZ();
localizer_pose_msg.pose.orientation.x = (local_transform * transform).getRotation().x();
localizer_pose_msg.pose.orientation.y = (local_transform * transform).getRotation().y();
localizer_pose_msg.pose.orientation.z = (local_transform * transform).getRotation().z();
localizer_pose_msg.pose.orientation.w = (local_transform * transform).getRotation().w();
}
else
{
localizer_pose_msg.header.frame_id = "/map";
localizer_pose_msg.header.stamp = current_scan_time;
localizer_pose_msg.pose.position.x = localizer_pose.x;
localizer_pose_msg.pose.position.y = localizer_pose.y;
localizer_pose_msg.pose.position.z = localizer_pose.z;
localizer_pose_msg.pose.orientation.x = localizer_q.x();
localizer_pose_msg.pose.orientation.y = localizer_q.y();
localizer_pose_msg.pose.orientation.z = localizer_q.z();
localizer_pose_msg.pose.orientation.w = localizer_q.w();
}
predict_pose_pub.publish(predict_pose_msg);
ndt_pose_pub.publish(ndt_pose_msg);
// current_pose is published by vel_pose_mux
// current_pose_pub.publish(current_pose_msg);
localizer_pose_pub.publish(localizer_pose_msg);
// Send TF "/base_link" to "/map"
// base_link到map原点的tf变换
transform.setOrigin(tf::Vector3(current_pose.x, current_pose.y, current_pose.z));
transform.setRotation(current_q);
// br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_footprint"));
if (_use_local_transform == true)
{
br.sendTransform(tf::StampedTransform(local_transform * transform, current_scan_time, "/map", "/base_footprint"));
}
else
{
//发步tf变换
br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_footprint"));
}
//计算匹配所需要的时间
matching_end = std::chrono::system_clock::now();
exe_time = std::chrono::duration_cast<std::chrono::microseconds>(matching_end - matching_start).count() / 1000.0;
time_ndt_matching.data = exe_time;
time_ndt_matching_pub.publish(time_ndt_matching);
// Set values for /estimate_twist
//发布底盘的运动速度
estimate_twist_msg.header.stamp = current_scan_time;
estimate_twist_msg.header.frame_id = "/base_link";
estimate_twist_msg.twist.linear.x = current_velocity;
estimate_twist_msg.twist.linear.y = 0.0;
estimate_twist_msg.twist.linear.z = 0.0;
estimate_twist_msg.twist.angular.x = 0.0;
estimate_twist_msg.twist.angular.y = 0.0;
estimate_twist_msg.twist.angular.z = angular_velocity;
estimate_twist_pub.publish(estimate_twist_msg);
geometry_msgs::Vector3Stamped estimate_vel_msg;
estimate_vel_msg.header.stamp = current_scan_time;
estimate_vel_msg.vector.x = current_velocity;
estimated_vel_pub.publish(estimate_vel_msg);
// Set values for /ndt_stat
//发布这次匹配的中的数据值
ndt_stat_msg.header.stamp = current_scan_time;//当前雷达的时间戳
ndt_stat_msg.exe_time = time_ndt_matching.data;//此次匹配所花时间
ndt_stat_msg.iteration = iteration;//迭代次数
ndt_stat_msg.score = fitness_score;//匹配分数
ndt_stat_msg.velocity = current_velocity;//当前速度
ndt_stat_msg.acceleration = current_accel;//当前加速度
ndt_stat_msg.use_predict_pose = 0;//匹配的数据与上次数据的差异值是否过大
ndt_stat_pub.publish(ndt_stat_msg);
/* Compute NDT_Reliability */
//此次ndt匹配的可靠性
ndt_reliability.data = Wa * (exe_time / 100.0) * 100.0 + Wb * (iteration / 10.0) * 100.0 +
Wc * ((2.0 - trans_probability) / 2.0) * 100.0;
ndt_reliability_pub.publish(ndt_reliability);
// Write log
//ndt日志
if(_output_log_data)
{
if (!ofs)
{
std::cerr << "Could not open " << filename << "." << std::endl;
}
else
{
ofs << input->header.seq << "," << scan_points_num << "," << step_size << "," << trans_eps << "," << std::fixed
<< std::setprecision(5) << current_pose.x << "," << std::fixed << std::setprecision(5) << current_pose.y << ","
<< std::fixed << std::setprecision(5) << current_pose.z << "," << current_pose.roll << "," << current_pose.pitch
<< "," << current_pose.yaw << "," << predict_pose.x << "," << predict_pose.y << "," << predict_pose.z << ","
<< predict_pose.roll << "," << predict_pose.pitch << "," << predict_pose.yaw << ","
<< current_pose.x - predict_pose.x << "," << current_pose.y - predict_pose.y << ","
<< current_pose.z - predict_pose.z << "," << current_pose.roll - predict_pose.roll << ","
<< current_pose.pitch - predict_pose.pitch << "," << current_pose.yaw - predict_pose.yaw << ","
<< predict_pose_error << "," << iteration << "," << fitness_score << "," << trans_probability << ","
<< ndt_reliability.data << "," << current_velocity << "," << current_velocity_smooth << "," << current_accel
<< "," << angular_velocity << "," << time_ndt_matching.data << "," << align_time << "," << getFitnessScore_time
<< std::endl;
}
}
std::cout << "-----------------------------------------------------------------" << std::endl;
std::cout << "Sequence: " << input->header.seq << std::endl;
std::cout << "Timestamp: " << input->header.stamp << std::endl;
std::cout << "Frame ID: " << input->header.frame_id << std::endl;
// std::cout << "Number of Scan Points: " << scan_ptr->size() << " points." << std::endl;
std::cout << "Number of Filtered Scan Points: " << scan_points_num << " points." << std::endl;
std::cout << "NDT has converged: " << has_converged << std::endl;
std::cout << "Fitness Score: " << fitness_score << std::endl;
std::cout << "Transformation Probability: " << trans_probability << std::endl;
std::cout << "Execution Time: " << exe_time << " ms." << std::endl;
std::cout << "Number of Iterations: " << iteration << std::endl;
std::cout << "NDT Reliability: " << ndt_reliability.data << std::endl;
std::cout << "(x,y,z,roll,pitch,yaw): " << std::endl;
std::cout << "(" << current_pose.x << ", " << current_pose.y << ", " << current_pose.z << ", " << current_pose.roll
<< ", " << current_pose.pitch << ", " << current_pose.yaw << ")" << std::endl;
std::cout << "Transformation Matrix: " << std::endl;
std::cout << t << std::endl;
std::cout << "Align time: " << align_time << std::endl;
std::cout << "Get fitness score time: " << getFitnessScore_time << std::endl;
std::cout << "-----------------------------------------------------------------" << std::endl;
offset_imu_x = 0.0;
offset_imu_y = 0.0;
offset_imu_z = 0.0;
offset_imu_roll = 0.0;
offset_imu_pitch = 0.0;
offset_imu_yaw = 0.0;
offset_odom_x = 0.0;
offset_odom_y = 0.0;
offset_odom_z = 0.0;
offset_odom_roll = 0.0;
offset_odom_pitch = 0.0;
offset_odom_yaw = 0.0;
offset_imu_odom_x = 0.0;
offset_imu_odom_y = 0.0;
offset_imu_odom_z = 0.0;
offset_imu_odom_roll = 0.0;
offset_imu_odom_pitch = 0.0;
offset_imu_odom_yaw = 0.0;
// Update previous_***
//更新上一时刻数据值
previous_pose.x = current_pose.x;
previous_pose.y = current_pose.y;
previous_pose.z = current_pose.z;
previous_pose.roll = current_pose.roll;
previous_pose.pitch = current_pose.pitch;
previous_pose.yaw = current_pose.yaw;
previous_scan_time = current_scan_time;
previous_previous_velocity = previous_velocity;
previous_velocity = current_velocity;
previous_velocity_x = current_velocity_x;
previous_velocity_y = current_velocity_y;
previous_velocity_z = current_velocity_z;
previous_accel = current_accel;
previous_estimated_vel_kmph.data = estimated_vel_kmph.data;
}
}