本文旨在帮助读者利用自己的数据包完成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){ //初始化地图的变换参数，如无变换，默认全为0
nh.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_); // translation
Eigen::AngleAxisf rot_x_m2w(tf_roll_, Eigen::Vector3f::UnitX()); // rotation
Eigen::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(); //transmatrix
pcl::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 map
ROS_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 filter
pcl::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);
}
// Publishers
filtered_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);
// Subscribers
ros::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();
// Publishers
sensor_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);
// Subscribers
initial_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_ptr
geometry_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体素，单位为m
const 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_ptr
ndt_new.setInputTarget(map_points_ptr); //将map_points_ptr所指点云作为ndt的目标点云
// create Thread
// detach
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());
// swap
ndt_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 Map
std::lock_guard<std::mutex> lock(ndt_map_mtx_);
const std::string sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id; //接收到的velodyne_points的frame_id
const 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 sensor
geometry_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_ptr
const 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 cloud
ndt_.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;
}
// align
Eigen::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 0
pre_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_matrix
Eigen::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_matrix
Eigen::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
// publish
geometry_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 pose
if (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 cloud
pcl::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_ptr
sensor_msgs::PointCloud2 sensor_points_mapTF_msg;
pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg);//将map坐标系下的velodyne_points转为ROS类型的点云，存到sensor_points_mapTF_msg
sensor_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_points
std_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 initialize
private_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_quaternion
transform_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中就能够看到定位结果了！
809
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
