ROS实战之重定位——NDT

ROS实战之重定位——NDT

目录

安装方法

拷贝到工作区，编译，运行，环境依赖同上一篇，Ubuntu16.04+ROS Kinetic+PCL+GTSAM，在运行前将要加载的点云地图放到map目录下，并命名为kaist02.pcd

cd ~/catkin_ws/src
git clone https://github.com/irapkaist/SC-LeGO-LOAM.git
cd …
catkin_make
source devel/setup.bash
roslaunch lego_loam run.launch

和上节一样，播放自己的数据集就可以啦，播放之前在rviz中先发布当前位姿，在起点画一个水平向左的箭头，一定要这样操作。

绘制轨迹

想在重定位结果中绘制出机器人的运动轨迹，需要添加一些代码，用的是nav_msgs/Path这个类型的消息来实现
在这里插入图片描述在 [ 你的目录路径 ] /src/ndt_localizer/nodes/ndt.cpp中进行如上图修改，添加的代码是230,231,232,236行，同时要记得在ndt.h头文件中申明变量，如下图所示

ros::Publisher path_pub;

在的构造函数中定义变量，构造函数就是NdtLocalizer::NdtLocalizer(ros::NodeHandle &nh, ros::NodeHandle &private_nh):nh_(nh), private_nh_(private_nh), tf2_listener_(tf2_buffer_)
{……}，定义如下

path_pub = nh_.advertise<nav_msgs::Path>("trajectory",1, true);

重新编译，运行即可
在rviz中订阅话题，ADD by Topic，选择path即可

结果

在这里插入图片描述紫色的就是绘制出的机器人运动轨迹了

参考

https://github.com/AbangLZU/ndt_localizer
https://blog.csdn.net/u013834525/article/details/80447931?utm_source=blogkpcl0