一. 模块介绍
ndt_matching在整个系统中负责定位工作，它输出的定位信息几乎被所有关键模块订阅。
虚线代表可选输入topic
关键function一般包含着大量代码注释！有助于理解整个功能模块！红色实现框起来的函数为最关键函数
二. 源码详解
关键输入：
// Subscribers
ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 10, initialpose_callback);//初始位恣
ros::Subscriber points_sub = nh.subscribe("filtered_points", _queue_size, points_callback);//当前输入scan
关键输出：
ndt_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 10);//base_link在map坐标系下的位姿
localizer_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose", 10);//雷达坐标系在map坐标系下的位姿
单独起了一个线程来发布map：
//在thread_func中来加载map
pthread_create(&thread, NULL, thread_func, NULL);
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();
}
return nullptr;
}
map_callback回调函数
static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
// if (map_loaded == 0)
if (points_map_num != input->width)
{
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);
//tf-tree监控，指定map坐标系和world坐标系的变换关系
if (_use_local_transform == true)
{
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());
}
//大多数code和ndt_mapping是一样的，详情见2.ndt_mapping模块介绍及代码解析
pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(map));
// Setting point cloud to be aligned to.
if (_method_type == MethodType::PCL_GENERIC)
{
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> new_ndt;
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
new_ndt.setResolution(ndt_res);
new_ndt.setInputTarget(map_ptr);
new_ndt.setMaximumIterations(max_iter);
new_ndt.setStepSize(step_size);
new_ndt.setTransformationEpsilon(trans_eps);
new_ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
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;//地图标志位
}
}
points_callback回调函数
static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
health_checker_ptr_->CHECK_RATE("topic_rate_filtered_points_slow", 8, 5, 1, "topic filtered_points subscribe rate slow.");
// map成功load，并且init_pos被设定后才会执行此分支（可以通过gnss或者手动两种方式来设定init_pos）
if (map_loaded == 1 && init_pos_set == 1)
{
matching_start = std::chrono::system_clock::now();
static tf::TransformBroadcaster br;
tf::Transform transform;
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;
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)
{
align_start = std::chrono::system_clock::now();
//通过这个函数来进行ndt的求解
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();
//ndt求解完成后输出的值，判断ndt求解的准不准
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;
//坐标系转换，雷达在map坐标系转换成base_link坐标系下
t2 = t * tf_btol.inverse();
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
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)
{
use_predict_pose = 0;
}
else
{
use_predict_pose = 1;
}
use_predict_pose = 0;
if (use_predict_pose == 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
{
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
//当前pose和上一帧pose的变化值
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);
const pose trans_current_pose = convertPoseIntoRelativeCoordinate(current_pose, previous_pose);
current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
current_velocity = (trans_current_pose.x >= 0) ? current_velocity : -current_velocity;
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;
current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
if (std::fabs(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;
estimated_vel_mps.data = current_velocity;
estimated_vel_kmph.data = current_velocity * 3.6;
//发布话题m/s，km/h
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
{
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_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_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_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);
health_checker_ptr_->CHECK_RATE("topic_rate_ndt_pose_slow", 8, 5, 1, "topic ndt_pose publish rate slow.");
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"
//current_pose通过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_link"));
if (_use_local_transform == true)
{
br.sendTransform(tf::StampedTransform(local_transform * transform, current_scan_time, "/map", "/base_link"));
}
else
{
br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link"));
}
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;
health_checker_ptr_->CHECK_MAX_VALUE("time_ndt_matching", time_ndt_matching.data, 50, 70, 100, "value time_ndt_matching is too high.");
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;
health_checker_ptr_->CHECK_MAX_VALUE("estimate_twist_linear", current_velocity, 5, 10, 15, "value linear estimated twist is too high.");
health_checker_ptr_->CHECK_MAX_VALUE("estimate_twist_angular", angular_velocity, 5, 10, 15, "value linear angular twist is too high.");
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_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
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;
}
}
initialpose_callback回调函数
static void initialpose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& input)
{
tf::TransformListener listener;
tf::StampedTransform transform;
try
{
ros::Time now = ros::Time(0);
listener.waitForTransform("/map", input->header.frame_id, now, ros::Duration(10.0));
listener.lookupTransform("/map", input->header.frame_id, now, transform);
}
catch (tf::TransformException& ex)
{
ROS_ERROR("%s", ex.what());
}
tf::Quaternion q(input->pose.pose.orientation.x, input->pose.pose.orientation.y, input->pose.pose.orientation.z,
input->pose.pose.orientation.w);
tf::Matrix3x3 m(q);
if (_use_local_transform == true)//_use_local_transform需要进行world和map的坐标变换
{
current_pose.x = input->pose.pose.position.x;
current_pose.y = input->pose.pose.position.y;
current_pose.z = input->pose.pose.position.z;
}
else
{
// 要注意我们从rviz手动给的pose是基于world坐标系的，所以需要将其转换到map坐标系
current_pose.x = input->pose.pose.position.x + transform.getOrigin().x();
current_pose.y = input->pose.pose.position.y + transform.getOrigin().y();
current_pose.z = input->pose.pose.position.z + transform.getOrigin().z();
}
m.getRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
// 这里主要就是为了确定一个z值
if (_get_height == true && map_loaded == 1)
{
double min_distance = DBL_MAX;
double nearest_z = current_pose.z;
for (const auto& p : map)
{
double distance = hypot(current_pose.x - p.x, current_pose.y - p.y);
if (distance < min_distance)
{
min_distance = distance;
nearest_z = p.z;
}
}
current_pose.z = nearest_z;
}
current_pose_imu = current_pose_odom = current_pose_imu_odom = current_pose;
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;
current_velocity = 0.0;
current_velocity_x = 0.0;
current_velocity_y = 0.0;
current_velocity_z = 0.0;
angular_velocity = 0.0;
current_accel = 0.0;
current_accel_x = 0.0;
current_accel_y = 0.0;
current_accel_z = 0.0;
offset_x = 0.0;
offset_y = 0.0;
offset_z = 0.0;
offset_yaw = 0.0;
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;
init_pos_set = 1;
}