PL-ICP帧间匹配lesson3

一、scan_tools  lesson3 scan_match_plicp

本篇文章对应的代码为lesson3  

ros中有人使用使用csm包进行了ros下的实现,进行了扫描匹配与位姿累加的实现,但是没有发布odometry的topic与tf.包的名字为 laser_scan_matcher,是scan_tools包集中的一个.

首先介绍一下scan_tools包集,这个包里提供了很多操作二维激光雷达数据的功能,虽然不一定能直接用,但是其处理数据的思路是非常值得借鉴的.其wiki的地址为: scan_tools - ROS Wiki

1.2 功能简介 

其包含的功能包名字如下所示,并对其功能进行了简要介绍.

laser_ortho_projector: 将切斜的雷达数据投影到平面上.
laser_scan_matcher: 基于pl-icp的扫描匹配的实现,并进行了位姿累加
laser_scan_sparsifier: 对雷达数据进行稀疏处理
laser_scan_splitter: 将一帧雷达数据分段,并发布出去
ncd_parser: 读取New College Dataset,转换成ros的scan 与 odometry 发布出去
polar_scan_matcher: 基于Polar Scan Matcher的扫描匹配器的ros实现
scan_to_cloud_converter: 将 sensor_msgs/LaserScan 数据转成 sensor_msgs/PointCloud2 的数据格式.
楼主前的那篇将雷达数据转成 sensor_msgs/PointCloud2 的文章,就是参考scan_to_cloud_converter包来实现的.

如果想要深入学习一下的请去wiki的网址,看介绍及源码,这里就不再过多介绍了.

1.3依赖

sudo apt-get install ros-noetic-csm

1.4CMakeLists

需要在CMakeLists.txt中添加如下内容,以在程序中使用csm

# Find csm project
find_package(PkgConfig)
pkg_check_modules(csm REQUIRED csm)
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${csm_INCLUDE_DIRS} 
)
link_directories(${csm_LIBRARY_DIRS})

#Create library
add_executable(${PROJECT_NAME}_scan_match_plicp_node src/scan_match_plicp.cc)

#Note we don't link against pcl as we're using header-only parts of the library
target_link_libraries( ${PROJECT_NAME}_scan_match_plicp_node 
${catkin_LIBRARIES} 
${csm_LIBRARIES}
)

add_dependencies(${PROJECT_NAME}_scan_match_plicp_node 
${csm_EXPORTED_TARGETS} 
${catkin_EXPORTED_TARGETS}
)

1.5 csm算法 

scan_match_plicp 

1.5 .1回调函数

首先要进行初始化,初始化的工作做了第一帧雷达数据的赋值,第一帧时间的赋值.

并且计算了雷达数据对应的每个角度值的cos与sin,用于节省计算量.

之后再将雷达数据转换成csm需要的格式

1.6

类中的私有成员函数, 可以 通过类中的公共函数 来调用其他的私有成员函数

私有成员函数特性, 类外不能直接调用, 类内可以访问

一般封装的话    回调函数放在类的 public 中,  其他的放在private中 , 在回调函数Callback中访问  

//公共权限  public     类内可以访问  类外可以访问
//保护权限  protected  类内可以访问  类外不可以访问
//私有权限  private    类内可以访问  类外不可以访问

1.7编译和运行

由于是新增的包,所以需要通过 rospack profile 命令让ros找到这个新的包.

roslaunch lesson3 scan_match_plicp.launch

PL-ICP帧间匹配lesson3_第1张图片

 二、基于PL-ICP的激光雷达里程计 lesson3 plicp_odometry

1.TF2

1.1坐标系

map: 地图坐标系,也被称为世界坐标系,是静止不动的
odom: 里程计坐标系,相对于map来说一般情况下是静止的,有些情况下会变动(定位节点为了修正机器人的位姿从而改变了map->odom间的坐标变换)
base_link: 代表机器人的旋转中心的坐标系,相对于odom来说base_link是运动的
laser_link: 激光雷达的坐标系,相对于base_link来说是静止的,因为雷达装在机器人上,雷达不会自己飞起来

可以看到,上边的坐标系,是单方向依赖的, laser_link 依赖于 base_link,base_link 依赖于 odom 。也可以说成是一个坐标系指向下一个坐标系的,连起来的话也成了:

map -> odom -> base_link -> laser_link

tf2常用数据类型与常用函数汇总_李太白lx的博客-CSDN博客

使用私有句柄 private_node_ 获取节点内部参数,参数写在配置文件中,配置文件稍后讲解 

2、 lookupTransform

// 获取tf并不是瞬间就能获取到的,要给1秒的缓冲时间让其找到tf

虽然在tf树中可以看到各个坐标之间的已经存在,但是却 Buffer::lookupTransform()时报错,主要是因为在使用时,大家都默认不填写第四个参数timeout或填写的时间比较短而导致的。这个参数表示最长等待监听时间,如果没有这个参数,那么监听一次,没有查询到数据,就失败,但是,有时候lookupTransform()函数处理时间比较长,所以导致出错。
 

bool ScanMatchPLICP::GetBaseToLaserTf(const std::string &frame_id)
{
    ros::Time t = ros::Time::now();

    geometry_msgs::TransformStamped transformStamped;
    // 获取tf并不是瞬间就能获取到的,要给1秒的缓冲时间让其找到tf
    try
    {
        transformStamped = tfBuffer_.lookupTransform(base_frame_, frame_id,
                                                     t, ros::Duration(1.0));
    }
    catch (tf2::TransformException &ex)
    {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        return false;
    }

    // 将获取的tf存到base_to_laser_中
    tf2::fromMsg(transformStamped.transform, base_to_laser_);
    laser_to_base_ = base_to_laser_.inverse();

    return true;
}

3、 ScanMatchWithPLICP()

增加了 基于匀速模型的位姿预测,位姿累加,发布TF以及odom话题,新建关键帧,这4个功能

2、坐标转化

  if (output_.valid)
    {
        // 雷达坐标系下的坐标变换
        tf2::Transform corr_ch_l;
        CreateTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);

        // 将雷达坐标系下的坐标变换 转换成 base_link坐标系下的坐标变换
        corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;

        // 更新 base_link 在 odom 坐标系下 的坐标
        base_in_odom_ = base_in_odom_keyframe_ * corr_ch;

        latest_velocity_.linear.x = corr_ch.getOrigin().getX() / dt;
        latest_velocity_.angular.z = tf2::getYaw(corr_ch.getRotation()) / dt;
    }

PL-ICP帧间匹配lesson3_第2张图片

3运行

3.1 生成新bag文件

1、由于 lesson1 这个数据包中已经有了tf,而本篇文章的代码也要发布TF,为了让流程更加清晰,现在我重新录制一个没有tf的数据包.通过分别在3个终端中执行如下命令进行新bag的录制

2也可以不重新录制包,只要将我们代码的TF的odom坐标系,改个名字如odom_plicp,再将Rviz的Fixed Frame设置成odom_plicp也可以

# 终端1
cd ~/bagfiles
roscore
# 终端2
rosparam set use_sim_time true
cd ~/bagfiles
rosbag play --clock lesson1.bag
# 终端3
cd ~/bagfiles
rosbag record /laser_scan /odom
 

use_sim_time

这个参数当回放bag数据集是设置为true,此时说明系统使用的是仿真时间Time;如果设置为false,则系统使用walltime。(此参数可以通过launch文件中设置,或者通过节点设置)

  • 发布时钟
    默认/clock话题是没有消息的。

rosbag play --clock bag_name.bag #默认/clock的话题频率100hz rosbag play --clock --hz 200 bag_name.bag #指定/clock的时钟为200hz

三、ros

1、roll,pitch,yall

 //位姿信息中没有四元数, 但有偏航角度,又已知乌龟是2d,没有翻滚与付仰, 所以为 0  0  角度   ,偏航
   tf2::Quaternion qtn;
   qtn.setRPY(0,0,pose->theta);

2、nav_msgs geometry_msgs 两者区别

【ROS】rosbag使用_Amelie_xiao的博客-CSDN博客_rosbag

PL-ICP帧间匹配lesson3_第3张图片

PL-ICP帧间匹配lesson3_第4张图片

红色轨迹为通过小车自身编码器累加出来的里程计,黄色轨迹为通过PL-ICP算法累加出来的激光雷达里程计

你可能感兴趣的:(机器人,自动驾驶,人工智能)