本文主要是介绍利用自己的数据包实现点云地图的NDT定位，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
本文旨在帮助读者利用自己的数据包完成3D点云地图下的定位，公开数据包NDT：自动驾驶系统进阶与项目实战（六）基于NDT的自动驾驶高精度定位和ROS项目实战，本文只讲应用。
废话不多说，直接开始！
一、安装ndt_localizer
#创建工作空间
mkdir -p ~/ndt_localizer/src
cd ~/ndt_localizer/src
catkin_init_workspace#克隆代码
git clone https://github.com/AbangLZU/ndt_localizer.git#编译
cd ..
catkin_makemaploader.cpp:
#include "mapLoader.h"MapLoader::MapLoader(ros::NodeHandle &nh){ //加载pcd地图std::string pcd_file_path, map_topic;nh.param<std::string>("pcd_path", pcd_file_path, "");//"pcd_pcth" 和 "map_topic" 可以在launch文件中修改nh.param<std::string>("map_topic", map_topic, "point_map");init_tf_params(nh);pc_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>(map_topic, 10, true);//发布获取到的pcd文件里的点云话题file_list_.push_back(pcd_file_path);auto pc_msg = CreatePcd();auto out_msg = TransformMap(pc_msg);if (out_msg.width != 0) {out_msg.header.frame_id = "map";pc_map_pub_.publish(out_msg);}}void MapLoader::init_tf_params(ros::NodeHandle &nh){ //初始化地图的变换参数，如无变换，默认全为0nh.param<float>("x", tf_x_, 0.0);nh.param<float>("y", tf_y_, 0.0);nh.param<float>("z", tf_z_, 0.0);nh.param<float>("roll", tf_roll_, 0.0);nh.param<float>("pitch", tf_pitch_, 0.0);nh.param<float>("yaw", tf_yaw_, 0.0);ROS_INFO_STREAM("x" << tf_x_ <<"y: "<<tf_y_<<"z: "<<tf_z_<<"roll: "<<tf_roll_<<" pitch: "<< tf_pitch_<<"yaw: "<<tf_yaw_);
}sensor_msgs::PointCloud2 MapLoader::TransformMap(sensor_msgs::PointCloud2 & in){ //执行初始地图变换pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc(new pcl::PointCloud<pcl::PointXYZ>);pcl::fromROSMsg(in, *in_pc);pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>);Eigen::Translation3f tl_m2w(tf_x_, tf_y_, tf_z_); // translationEigen::AngleAxisf rot_x_m2w(tf_roll_, Eigen::Vector3f::UnitX()); // rotationEigen::AngleAxisf rot_y_m2w(tf_pitch_, Eigen::Vector3f::UnitY());Eigen::AngleAxisf rot_z_m2w(tf_yaw_, Eigen::Vector3f::UnitZ());Eigen::Matrix4f tf_m2w = (tl_m2w * rot_z_m2w * rot_y_m2w * rot_x_m2w).matrix(); //transmatrixpcl::transformPointCloud(*in_pc, *transformed_pc_ptr, tf_m2w);//将点云*in_pc通过tf_m2w变换并保存到*transformed_pc_ptr中SaveMap(transformed_pc_ptr);//将变换后的地图保存在"/tmp/transformed_map.pcd"sensor_msgs::PointCloud2 output_msg;pcl::toROSMsg(*transformed_pc_ptr, output_msg);//将*transformed_pc_ptr点云转换为ROS格式return output_msg;
}void MapLoader::SaveMap(const pcl::PointCloud<pcl::PointXYZ>::Ptr map_pc_ptr){ //定义修改后的地图保存函数pcl::io::savePCDFile("/tmp/transformed_map.pcd", *map_pc_ptr);
}sensor_msgs::PointCloud2 MapLoader::CreatePcd() //用于加载pcd文件
{sensor_msgs::PointCloud2 pcd, part;for (const std::string& path : file_list_) {// Following outputs are used for progress bar of Runtime Manager.if (pcd.width == 0) { if (pcl::io::loadPCDFile(path.c_str(), pcd) == -1) {std::cerr << "load failed " << path << std::endl;}} else {if (pcl::io::loadPCDFile(path.c_str(), part) == -1) {std::cerr << "load failed " << path << std::endl;}pcd.width += part.width;pcd.row_step += part.row_step;pcd.data.insert(pcd.data.end(), part.data.begin(), part.data.end());}std::cerr << "load " << path << std::endl;if (!ros::ok()) break;}return pcd;
}int main(int argc, char** argv)
{ros::init(argc, argv, "map_loader"); //utilize this node to launch pointcloud mapROS_INFO("\033[1;32m---->\033[0m Map Loader Started.");ros::NodeHandle nh("~");MapLoader map_loader(nh);ros::spin();return 0;
}
voxel_grid_filter.cpp:
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>#include "points_downsampler.h"#define MAX_MEASUREMENT_RANGE 120.0 //定义最大侦测距离ros::Publisher filtered_points_pub;// Leaf size of VoxelGrid filter.
static double voxel_leaf_size = 0.3;static bool _output_log = false;
static std::ofstream ofs;
static std::string filename;static std::string POINTS_TOPIC;
static double measurement_range = MAX_MEASUREMENT_RANGE; static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{pcl::PointCloud<pcl::PointXYZ> scan;pcl::fromROSMsg(*input, scan); //将输入点云转为ROS格式if(measurement_range != MAX_MEASUREMENT_RANGE){scan = removePointsByRange(scan, 0, measurement_range); //removepointByRange函数在points_downsampleer.h中定义, 对输入的点云，去除超过最大侦测距离的点}pcl::PointCloud<pcl::PointXYZ>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(scan));pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());sensor_msgs::PointCloud2 filtered_msg;//如果体素滤波体素不小于0.1，执行滤波 if (voxel_leaf_size >= 0.1){// Downsampling the velodyne points using VoxelGrid filterpcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);voxel_grid_filter.setInputCloud(scan_ptr);voxel_grid_filter.filter(*filtered_scan_ptr);pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);}//如果体素滤波体素小于0.1，不执行滤波 else{pcl::toROSMsg(*scan_ptr, filtered_msg);}filtered_msg.header = input->header;filtered_points_pub.publish(filtered_msg); //发布降采样后的点云}int main(int argc, char** argv)
{ros::init(argc, argv, "voxel_grid_filter");ros::NodeHandle nh;ros::NodeHandle private_nh("~");private_nh.getParam("points_topic", POINTS_TOPIC);private_nh.getParam("output_log", _output_log);private_nh.param<double>("leaf_size", voxel_leaf_size, 0.3);ROS_INFO_STREAM("Voxel leaf size is: "<<voxel_leaf_size);if(_output_log == true){char buffer[80];std::time_t now = std::time(NULL);std::tm *pnow = std::localtime(&now);std::strftime(buffer,80,"%Y%m%d_%H%M%S",pnow);filename = "voxel_grid_filter_" + std::string(buffer) + ".csv";ofs.open(filename.c_str(), std::ios::app);}// Publishersfiltered_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);// Subscribersros::Subscriber scan_sub = nh.subscribe(POINTS_TOPIC, 10, scan_callback);ros::spin();return 0;
}
ndt.cpp:
#include "ndt.h"NdtLocalizer::NdtLocalizer(ros::NodeHandle &nh, ros::NodeHandle &private_nh):nh_(nh), private_nh_(private_nh), tf2_listener_(tf2_buffer_){//tf2_listener_(tf2_buffer_)：通过连接接收tf2转换key_value_stdmap_["state"] = "Initializing";init_params();// Publisherssensor_aligned_pose_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("points_aligned", 10);ndt_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("ndt_pose", 10);exe_time_pub_ = nh_.advertise<std_msgs::Float32>("exe_time_ms", 10);transform_probability_pub_ = nh_.advertise<std_msgs::Float32>("transform_probability", 10);iteration_num_pub_ = nh_.advertise<std_msgs::Float32>("iteration_num", 10);diagnostics_pub_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 10);// Subscribersinitial_pose_sub_ = nh_.subscribe("initialpose", 100, &NdtLocalizer::callback_init_pose, this); //从rviz中获取初始位姿信息 map_points_sub_ = nh_.subscribe("points_map", 1, &NdtLocalizer::callback_pointsmap, this); //从maploader中获取的pcd点云文件话题sensor_points_sub_ = nh_.subscribe("filtered_points", 1, &NdtLocalizer::callback_pointcloud, this);//从voxel_grid_filter获取的降采样后的点云diagnostic_thread_ = std::thread(&NdtLocalizer::timer_diagnostic, this);diagnostic_thread_.detach();
}NdtLocalizer::~NdtLocalizer() {}void NdtLocalizer::timer_diagnostic()
{ros::Rate rate(100);while (ros::ok()) {diagnostic_msgs::DiagnosticStatus diag_status_msg;diag_status_msg.name = "ndt_scan_matcher";diag_status_msg.hardware_id = "";for (const auto & key_value : key_value_stdmap_) {diagnostic_msgs::KeyValue key_value_msg;key_value_msg.key = key_value.first;key_value_msg.value = key_value.second;diag_status_msg.values.push_back(key_value_msg);}diag_status_msg.level = diagnostic_msgs::DiagnosticStatus::OK;diag_status_msg.message = "";if (key_value_stdmap_.count("state") && key_value_stdmap_["state"] == "Initializing") {diag_status_msg.level = diagnostic_msgs::DiagnosticStatus::WARN;diag_status_msg.message += "Initializing State. ";}if (key_value_stdmap_.count("skipping_publish_num") &&std::stoi(key_value_stdmap_["skipping_publish_num"]) > 1) {diag_status_msg.level = diagnostic_msgs::DiagnosticStatus::WARN;diag_status_msg.message += "skipping_publish_num > 1. ";}if (key_value_stdmap_.count("skipping_publish_num") &&std::stoi(key_value_stdmap_["skipping_publish_num"]) >= 5) {diag_status_msg.level = diagnostic_msgs::DiagnosticStatus::ERROR;diag_status_msg.message += "skipping_publish_num exceed limit. ";}diagnostic_msgs::DiagnosticArray diag_msg;diag_msg.header.stamp = ros::Time::now();diag_msg.status.push_back(diag_status_msg);diagnostics_pub_.publish(diag_msg);rate.sleep();}
}//这个函数用于对初始位姿变换到map坐标系下，并用initial_pose_cov_msg_表示
void NdtLocalizer::callback_init_pose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & initial_pose_msg_ptr) //从rviz中获取到初始位姿后调用，输入为初始位姿信息
{if (initial_pose_msg_ptr->header.frame_id == map_frame_) { //如果输入的位姿是“map”坐标系下的，将其赋值给initial_pose_cov_msg_initial_pose_cov_msg_ = *initial_pose_msg_ptr;} else { //如果输入的位姿不是“map”坐标系下，将其转到“map”坐标系下后再赋值给initial_pose_cov_msg_ // 获取位姿坐标系与“map”坐标系之间的tf变换，并存到TF_pose_to_map_ptrgeometry_msgs::TransformStamped::Ptr TF_pose_to_map_ptr(new geometry_msgs::TransformStamped);get_transform(map_frame_, initial_pose_msg_ptr->header.frame_id, TF_pose_to_map_ptr); //获取位姿坐标系到map的tf变换，并存到TF_pose_to_map_ptr// 利用TF_pose_to_map_ptr将输入位姿由位姿坐标系转到“map”坐标系，新位姿由*mapTF_initial_pose_msg_ptr表示geometry_msgs::PoseWithCovarianceStamped::Ptr mapTF_initial_pose_msg_ptr(new geometry_msgs::PoseWithCovarianceStamped);tf2::doTransform(*initial_pose_msg_ptr, *mapTF_initial_pose_msg_ptr, *TF_pose_to_map_ptr); // mapTF_initial_pose_msg_ptr->header.stamp = initial_pose_msg_ptr->header.stamp;initial_pose_cov_msg_ = *mapTF_initial_pose_msg_ptr; //将转到“map”坐标系后的位姿赋值给initial_pose_cov_msg_}// if click the initpose again, re init！init_pose = false;
}//用于将pcd文件点云设置为ndt的目标点云，设置ndt各参数
void NdtLocalizer::callback_pointsmap( const sensor_msgs::PointCloud2::ConstPtr & map_points_msg_ptr)//从maploader节点获取到pcd文件的点云话题后调用，输入为pcd文件内的点云
{
//ndt_ 在ndt.h中定义: pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_;const auto trans_epsilon = ndt_.getTransformationEpsilon(); //最小搜索变化量const auto step_size = ndt_.getStepSize(); // 搜索步长const auto resolution = ndt_.getResolution(); //目标点云的ND体素，单位为mconst auto max_iterations = ndt_.getMaximumIterations();//使用牛顿法优化的迭代次数pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_new;ndt_new.setTransformationEpsilon(trans_epsilon);// 更新ndt_ndt_new.setStepSize(step_size);ndt_new.setResolution(resolution);ndt_new.setMaximumIterations(max_iterations);pcl::PointCloud<pcl::PointXYZ>::Ptr map_points_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr); //将输入的pcd文件内的ROS类型点云转到*map_points_ptrndt_new.setInputTarget(map_points_ptr); //将map_points_ptr所指点云作为ndt的目标点云// create Thread// detachpcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());// swapndt_map_mtx_.lock();ndt_ = ndt_new;ndt_map_mtx_.unlock();
}void NdtLocalizer::callback_pointcloud( const sensor_msgs::PointCloud2::ConstPtr & sensor_points_sensorTF_msg_ptr)// 从voxel_grid_filter节点获取到降采样后的velodyne点云信息后调用，输入为降采样后的velodyne_points
{const auto exe_start_time = std::chrono::system_clock::now();// mutex Mapstd::lock_guard<std::mutex> lock(ndt_map_mtx_);const std::string sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id; //接收到的velodyne_points的frame_idconst auto sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp; //接收到velodyne_points的时间戳boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_sensorTF_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::fromROSMsg(*sensor_points_sensorTF_msg_ptr, *sensor_points_sensorTF_ptr);//将接收的到ROS类型的velodyne_points点云转到*sensor_points_sensorTF_ptr// get TF base to sensorgeometry_msgs::TransformStamped::Ptr TF_base_to_sensor_ptr(new geometry_msgs::TransformStamped);get_transform(base_frame_, sensor_frame, TF_base_to_sensor_ptr); //获取velodyne到base的tf变换，并存到TF_base_to_sensor_ptrconst Eigen::Affine3d base_to_sensor_affine = tf2::transformToEigen(*TF_base_to_sensor_ptr);const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast<float>();//从velodyne到base的转换矩阵boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_baselinkTF_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix);//将velodyne_points点云通过base_to_sensor_matrix转换到base坐标系，结果保存到*sensor_points_baselinkTF_ptr// set input point cloudndt_.setInputSource(sensor_points_baselinkTF_ptr);//将base坐标系下的velodyne_points设置为ndt的输入源; (the ndt target is the modified pointcloud in pcd map)if (ndt_.getInputTarget() == nullptr) {ROS_WARN_STREAM_THROTTLE(1, "No MAP!");return;}// alignEigen::Matrix4f initial_pose_matrix;if (!init_pose){Eigen::Affine3d initial_pose_affine;tf2::fromMsg(initial_pose_cov_msg_.pose.pose, initial_pose_affine); //将rviz中获取到的ROS类型的初始位姿转为矩阵形式initial_pose_matrix = initial_pose_affine.matrix().cast<float>();// for the first time, we don't know the pre_trans, so just use the init_trans, // which means, the delta trans for the second time is 0pre_trans = initial_pose_matrix;init_pose = true;}else{// 将上一帧求得的位姿作为初始位姿，利用线性模型做当前帧位姿的估计initial_pose_matrix = pre_trans * delta_trans;}pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);const auto align_start_time = std::chrono::system_clock::now();key_value_stdmap_["state"] = "Aligning";ndt_.align(*output_cloud, initial_pose_matrix);key_value_stdmap_["state"] = "Sleeping";const auto align_end_time = std::chrono::system_clock::now();const double align_time = std::chrono::duration_cast<std::chrono::microseconds>(align_end_time - align_start_time).count() /1000.0; //ndt配准总时长const Eigen::Matrix4f result_pose_matrix = ndt_.getFinalTransformation();//将ndt配准的最终结果转为矩阵形式，存到result_pose_matrixEigen::Affine3d result_pose_affine;result_pose_affine.matrix() = result_pose_matrix.cast<double>();const geometry_msgs::Pose result_pose_msg = tf2::toMsg(result_pose_affine); //将ndt最终结果转为ROS类型并发布const auto exe_end_time = std::chrono::system_clock::now();const double exe_time = std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count() / 1000.0;const float transform_probability = ndt_.getTransformationProbability();const int iteration_num = ndt_.getFinalNumIteration();//收敛判别bool is_converged = true; static size_t skipping_publish_num = 0;if ( iteration_num >= ndt_.getMaximumIterations() + 2 ||transform_probability < converged_param_transform_probability_) {is_converged = false;++skipping_publish_num;std::cout << "Not Converged" << std::endl;} else {skipping_publish_num = 0;}delta_trans = pre_trans.inverse() * result_pose_matrix; // 求用于下一帧线性计算需要的delta_trans=pre_trans^-1 * result_pose_matrixEigen::Vector3f delta_translation = delta_trans.block<3, 1>(0, 3);std::cout<<"delta x: "<<delta_translation(0) << " y: "<<delta_translation(1)<<" z: "<<delta_translation(2)<<std::endl;Eigen::Matrix3f delta_rotation_matrix = delta_trans.block<3, 3>(0, 0);Eigen::Vector3f delta_euler = delta_rotation_matrix.eulerAngles(2,1,0);std::cout<<"delta yaw: "<<delta_euler(0) << " pitch: "<<delta_euler(1)<<" roll: "<<delta_euler(2)<<std::endl;pre_trans = result_pose_matrix; //更新pre_trans// publishgeometry_msgs::PoseStamped result_pose_stamped_msg;result_pose_stamped_msg.header.stamp = sensor_ros_time;//velodyne_points的接收时间戳result_pose_stamped_msg.header.frame_id = map_frame_;result_pose_stamped_msg.pose = result_pose_msg;//the result poseif (is_converged) {ndt_pose_pub_.publish(result_pose_stamped_msg);//如果收敛，发布带有frame_id和时间戳信息的配准结果}publish_tf(map_frame_, base_frame_, result_pose_stamped_msg); // publish tf(map frame to base frame)// publish aligned point cloudpcl::PointCloud<pcl::PointXYZ>::Ptr sensor_points_mapTF_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, result_pose_matrix);//将base坐标系下的velodyne_points通过result_pose_matrix转换到map坐标系下，保存到*sensor_points_mapTF_ptrsensor_msgs::PointCloud2 sensor_points_mapTF_msg;pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg);//将map坐标系下的velodyne_points转为ROS类型的点云，存到sensor_points_mapTF_msgsensor_points_mapTF_msg.header.stamp = sensor_ros_time;sensor_points_mapTF_msg.header.frame_id = map_frame_;sensor_aligned_pose_pub_.publish(sensor_points_mapTF_msg);//发布带有时间戳和frame_id的map坐标系下的velodyne_pointsstd_msgs::Float32 exe_time_msg;exe_time_msg.data = exe_time;exe_time_pub_.publish(exe_time_msg);std_msgs::Float32 transform_probability_msg;transform_probability_msg.data = transform_probability;transform_probability_pub_.publish(transform_probability_msg);std_msgs::Float32 iteration_num_msg;iteration_num_msg.data = iteration_num;iteration_num_pub_.publish(iteration_num_msg);key_value_stdmap_["seq"] = std::to_string(sensor_points_sensorTF_msg_ptr->header.seq);key_value_stdmap_["transform_probability"] = std::to_string(transform_probability);key_value_stdmap_["iteration_num"] = std::to_string(iteration_num);key_value_stdmap_["skipping_publish_num"] = std::to_string(skipping_publish_num);std::cout << "------------------------------------------------" << std::endl;std::cout << "align_time: " << align_time << "ms" << std::endl;std::cout << "exe_time: " << exe_time << "ms" << std::endl;std::cout << "trans_prob: " << transform_probability << std::endl;std::cout << "iter_num: " << iteration_num << std::endl;std::cout << "skipping_publish_num: " << skipping_publish_num << std::endl;
}void NdtLocalizer::init_params(){ //execute initializeprivate_nh_.getParam("base_frame", base_frame_);ROS_INFO("base_frame_id: %s", base_frame_.c_str());double trans_epsilon = ndt_.getTransformationEpsilon();double step_size = ndt_.getStepSize();double resolution = ndt_.getResolution();int max_iterations = ndt_.getMaximumIterations();private_nh_.getParam("trans_epsilon", trans_epsilon);private_nh_.getParam("step_size", step_size);private_nh_.getParam("resolution", resolution);private_nh_.getParam("max_iterations", max_iterations);map_frame_ = "map";ndt_.setTransformationEpsilon(trans_epsilon);ndt_.setStepSize(step_size);ndt_.setResolution(resolution);ndt_.setMaximumIterations(max_iterations);ROS_INFO("trans_epsilon: %lf, step_size: %lf, resolution: %lf, max_iterations: %d", trans_epsilon,step_size, resolution, max_iterations);private_nh_.getParam("converged_param_transform_probability", converged_param_transform_probability_);
}bool NdtLocalizer::get_transform(const std::string & target_frame, const std::string & source_frame,const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr, const ros::Time & time_stamp)
{if (target_frame == source_frame) {transform_stamped_ptr->header.stamp = time_stamp;transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return true;}try { //try-catch异常处理*transform_stamped_ptr =tf2_buffer_.lookupTransform(target_frame, source_frame, time_stamp);//可以获得两个坐标系之间转换的关系} catch (tf2::TransformException & ex) { //如果发生异常，则ROS_WARN("%s", ex.what());ROS_ERROR("Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());transform_stamped_ptr->header.stamp = time_stamp;transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return false;}return true;
}bool NdtLocalizer::get_transform(const std::string & target_frame, const std::string & source_frame,const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr)
{if (target_frame == source_frame) {transform_stamped_ptr->header.stamp = ros::Time::now();transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return true;}try {*transform_stamped_ptr =tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0), ros::Duration(1.0));//可以获得两个坐标系之间转换的关系} catch (tf2::TransformException & ex) { //如果发生异常，则ROS_WARN("%s", ex.what());ROS_ERROR("Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());transform_stamped_ptr->header.stamp = ros::Time::now();transform_stamped_ptr->header.frame_id = target_frame;transform_stamped_ptr->child_frame_id = source_frame;transform_stamped_ptr->transform.translation.x = 0.0;transform_stamped_ptr->transform.translation.y = 0.0;transform_stamped_ptr->transform.translation.z = 0.0;transform_stamped_ptr->transform.rotation.x = 0.0;transform_stamped_ptr->transform.rotation.y = 0.0;transform_stamped_ptr->transform.rotation.z = 0.0;transform_stamped_ptr->transform.rotation.w = 1.0;return false;}return true;
}void NdtLocalizer::publish_tf(const std::string & frame_id, const std::string & child_frame_id,const geometry_msgs::PoseStamped & pose_msg)
{geometry_msgs::TransformStamped transform_stamped;transform_stamped.header.frame_id = frame_id;transform_stamped.child_frame_id = child_frame_id;transform_stamped.header.stamp = pose_msg.header.stamp;transform_stamped.transform.translation.x = pose_msg.pose.position.x; //位移变换transform_stamped.transform.translation.y = pose_msg.pose.position.y;transform_stamped.transform.translation.z = pose_msg.pose.position.z;tf2::Quaternion tf_quaternion;tf2::fromMsg(pose_msg.pose.orientation, tf_quaternion); //将ROS格式的位姿旋转量转到tf_quaterniontransform_stamped.transform.rotation.x = tf_quaternion.x(); //旋转变换transform_stamped.transform.rotation.y = tf_quaternion.y();transform_stamped.transform.rotation.z = tf_quaternion.z();transform_stamped.transform.rotation.w = tf_quaternion.w();tf2_broadcaster_.sendTransform(transform_stamped);//广播子坐标系到父坐标系的变换
}int main(int argc, char **argv)
{ros::init(argc, argv, "ndt_localizer");ros::NodeHandle nh;ros::NodeHandle private_nh("~");NdtLocalizer ndt_localizer(nh, private_nh);ros::spin();return 0;
}二、配置文件
1. map文件
将自己的数据包所建的pcd文件放置在代码包里的map文件夹下；
将/launch/map_loader.launch中的"pcd_path"路径下的.pcd文件名改为自己的pcd文件名：xxx.pcd；
2. 模型文件
将/launch/ndt_localizer.launch中的<include file="$(find ndt_localizer)/launch/lexus.launch" />语句注释掉；
如果需要添加自己的urdf模型则需添加对应的launch文件；
3. 点云文件
将/launch/points_downsample.launch文件中的"points_topic"下的话题名改为自己的激光雷达点云话题名，笔者使用的是velodyne16，因此改为"/velodyne_points"；
将"leaf_size"的值根据自己的激光雷达线数及建图场景修改，velodyne16室内建图修改为0.25效果不错，室外场景或32线/64线大一些；
4. 坐标变换
将/launch/static_tf.launch中的"localizer_to_base_link"后的两坐标改为"0 0 0.1 0 0 0 base_link velodyne"（针对velodyne）；0.1表示激光雷达坐标到base_link的z轴距离；
三、启动定位程序
启动ndt_localizer节点：
#启动ndt定位节点
source ~/ndt_localizer/devel/setup.bash
roslaunch ndt_localizer ndt_localizer.launch等待地图加载完成后，启动rosbag：
#注意建图的bag与定位bag要一致
rosbag play xxx.bag最后在终端和rviz中就能够看到定位结果了！
这篇关于利用自己的数据包实现点云地图的NDT定位的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
