松灵机器人Scout mini小车运行lio-sam
在之前的工作中，我们已经实现了用小车搭载传感器，采用gmapping建图和navigation导航实现小车在2D环境中自主导航，但是实际我们采用的激光雷达多为三维激光雷达。因此决定采用lio-sam来建图。具体操作步骤如下。
1.下载雷达仿真
1.1下载激光雷达包
首先将VLP16线的激光雷达仿真包放入之前的工作空间中
git clone https://bitbucket.org/DataspeedInc/velodyne_simulator/src/master/
下载之后查看文件夹结构如下：
下载好VLP激光雷达仿真包之后重新catkin_make一次，生成激光雷达点云产生库链接。编译之后在/devel/lib文件夹下生成两个动态链接库
1.2添加雷达支架描述文件
为了操作方便，我们给激光雷达一个支架，使得激光雷达能够立在支架上面。在对应机器人的urdf文件夹下新建一个laser_support.xacro文件，写入以下内容：
雷达支架描述文件
<?xml version="1.0"?>
<robot name="laser_support" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 雷达支架 -->
<xacro:property name="support_length" value="0.30" /> <!-- 支架长度 -->
<xacro:property name="support_radius" value="0.025" /> <!-- 支架半径 -->
<xacro:property name="support_x_size" value="-0.2" /> <!-- 支架安装的x坐标 -->
<xacro:property name="support_y_size" value="0.0" /> <!-- 支架安装的y坐标 -->
<xacro:property name="support_z_size" value="${base_z_size}" /> <!-- 支架安装的z坐标:底盘高度 / 2 + 支架高度 / 2 -->
<xacro:property name="support_m" value="0.02" /> <!-- 支架质量 -->
<link name="support">
<visual>
<geometry>
<cylinder radius="${support_radius}" length="${support_length}" />
</geometry>
<origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
<material name="red">
<color rgba="0.8 0.2 0.0 0.8" />
</material>
</visual>
<collision>
<geometry>
<cylinder radius="${support_radius}" length="${support_length}" />
</geometry>
<origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
</collision>
<xacro:cylinder_inertial_matrix m="${support_m}" r="${support_radius}" h="${support_length}" />
</link>
<joint name="support2base_link" type="fixed">
<parent link="base_link" />
<child link="support" />
<origin xyz="${support_x_size} ${support_y_size} ${support_z_size}" />
</joint>
<gazebo reference="support">
<material>Gazebo/White</material>
</gazebo>
</robot>
1.3在机器人的urdf描述文件中添加激光雷达的描述(urdf/empty.urdf)
首先包含激光雷达的支架描述
然后添加雷达的两个属性描述变量
最后，包含激光雷达的描述文件，这里会用到上面两个属性变量
1.4 启动仿真
编写launch文件，启动rviz和gazebo查看机器人
<launch>
<arg name="model" />
<arg name="gui" default="False" />
<!-- Loading model files -->
<arg name = "model_xacro" default = "$(find scout_description)/urdf/mini.xacro" />
<param name="robot_description" command="$(find xacro)/xacro $(arg model_xacro)" />
<!-- <param name="robot_description" textfile="$(find scout_description)/urdf/scout_mini.urdf" /> -->
<param name="use_gui" value="$(arg gui)" />
<!-- Launch the joint state publisher -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!-- Launch the robot state publisher -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find scout_description)/rviz/scout_mini_model_display.rviz" />
<arg name="world_name" default="$(find scout_gazebo_sim)/worlds/clearpath_playpen.world"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
</include>
<node pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model scout -param robot_description" />
</launch>
运行launch文件后可以看到机器人携带小车如下
此时可以打开我们的话题，看看是否有点云输出
可以看到我们的点云是正常输出的。注意，我们我们为了在gazebo里面看到我们的机器人，所以没有在gazebo里面吧激光点云可视化出来，如果想要可视化可以通过修改 VLP-16.urdf.xacro 文件中这个地方
2.添加imu模块
我们需要跑的框架是LIO-SAM，需要用到IMU模块，因此我们还需要添加imu进入仿真中，同样的我们在机器人对应的urdf文件夹下新建一个imu.xacro文件，写入以下内容：
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro">
<xacro:macro name="imu" params="sensor_name parent_link *origin">
<xacro:property name="imu_offset_x" value="0" />
<xacro:property name="imu_offset_y" value="0" />
<xacro:property name="imu_offset_z" value="0.2" />
<xacro:property name="imu_size" value="0.05" />
<xacro:property name="imu_m" value="0.01" /> <!-- imu质量 -->
<!-- imu -->
<joint name="imutobase" type="fixed">
<!-- <origin xyz="${imu_offset_x} ${imu_offset_y} ${imu_offset_z}" rpy="0 0 0" /> -->
<xacro:insert_block name="origin" />
<parent link="${parent_link}"/>
<child link="imu_base"/>
</joint>
<link name="imu_base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="${imu_size} ${imu_size} ${imu_size}"/>
</geometry>
<material name= "black" >
<color rgba="1.0 0.0 0.0 0.6" />
</material>
</visual>
<collision>
<geometry>
<box size="${imu_size} ${imu_size} ${imu_size}" />
</geometry>
<origin xyz="0.0 0.0 0" rpy="0.0 0.0 0.0" />
</collision>
<xacro:Box_inertial_matrix m = "${imu_m}" l = "${imu_size}" w = "${imu_size}" h = "${imu_size}"/>
</link>
<!-- 被引用的link -->
<gazebo reference="imu_base">
<material>Gazebo/Bule</material>
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu/data</topicName>
<bodyName>imu_base</bodyName>
<updateRateHZ>100.0</updateRateHZ>
<gaussianNoise>0.01</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_base</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
</xacro:macro>
</robot>
然后在urdf/empty.urdf文件中包含这个模块
3.lio-sam仿真
安装依赖
$ sudo apt-get install -y ros-noetic-navigation
$ sudo apt-get install -y ros-noetic-robot-localization
$ sudo apt-get install -y ros-noetic-robot-state-publisher
安装GTSAM
$ git clone https://github.com/borglab/gtsam
$ cd gtsam
$ mkdir build && cd build
# 注意这里要加-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF这个选项，不然后面运行会报错
$ cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF ..
$ sudo make install -j8
$ sudo ln -s /usr/local/lib/libmetis-gtsam.so /usr/lib/libmetis-gtsam.so //修改链接库
编译LIO-SAM
$ cd ~/robot_ws/src
$ git clone https://github.com/TixiaoShan/LIO-SAM //不要用官网的lio-sam 用博主内的lio-sam
$ cd ..
$ catkin_make
4.运行结果
首先启动仿真环境
roslaunch scout_gazebo_sim scout_mini_playpen.launch
运行lio-sam
roslaunch lio_sam run.launch
键盘控制机器人移动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
5.遇到的一些问题
gazebo中机器人静止，rviz中反复横跳
原因：lio-sam会对点云进行下采样滤波，滤波体素设置太大了，匹配过程出现误差，导致机器人优化出来的位姿反复横跳。
解决： 因为我们这里是在室内建图，所以在lio-sam的配置文件中把体素大小设置小一些
运行时报错 error while loading shared libraries: libmetis-gtsam.so: cannot open shared object file: No such file or directory
解决: sudo ln -s /usr/local/lib/libmetis-gtsam.so /usr/lib/libmetis-gtsam.so
**雷达和imu的xacro文件中 xacro:cylinder_inertial_matrix 和 xacro:Box_inertial_matrix报错
添加对应的惯性矩阵文件
惯性矩阵文件
<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- Macro for inertia matrix -->
<xacro:macro name="sphere_inertial_matrix" params="m r">
<inertial>
<mass value="${m}" />
<inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
iyy="${2*m*r*r/5}" iyz="0"
izz="${2*m*r*r/5}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</inertial>
</xacro:macro>
<xacro:macro name="Box_inertial_matrix" params="m l w h">
<inertial>
<mass value="${m}" />
<inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
iyy="${m*(w*w + l*l)/12}" iyz= "0"
izz="${m*(w*w + h*h)/12}" />
</inertial>
</xacro:macro>
</robot>