记录一下学习过程,方便日后复用。
文章目录
前言
本文使用多线雷达转单线,并使用cartographer建图,博主先用velodyne的VLP -16雷达完成了一遍仿真,本篇博客用的velodyne HDL -32E,二者稍做修改即可。博主的配置为:ubuntu 20.04 + ROS noetic 。
一、搭建仿真环境
1、安装cartographer
博主使用的是RMUS比赛的docker镜像,自带cartographer,省去了安装的麻烦,若有想使用该镜像的,可以去查看GitHub:
ICRA-RM-Sim2Real/sim2real-install-guide-cn.md at main · AIR-DISCOVER/ICRA-RM-Sim2Real · GitHubhttps://github.com/AIR-DISCOVER/ICRA-RM-Sim2Real/blob/main/docker_client/sim2real-install-guide-cn.md只需要cartographer的话,只安装client就好,docker镜像地址: rmus2022/client Tags | Docker Hubhttps://hub.docker.com/r/rmus2022/client/tags
2、创建小车工作空间
(1) 选择合适的小车就好,没有特殊要求,我用的是tianbot_mini,后文主要用到的工作空间就是tianbot_mini_ws,不再赘述。GitHub链接如下:
GitHub - tianbot/tianbot_mini: TianbotMini ROS移动机器人学习平台,10分钟跑通机器人SLAM应用,自主导航尽在掌心之中。https://github.com/tianbot/tianbot_mini有些文件我做了适当更改,另外小车太小,我做了适当放大,根据自己情况来就好。
(2) 下载velodyne_simulator至工作空间的src中,地址:
GitHub - lmark1/velodyne_simulator: URDF description and Gazebo plugins to simulate Velodyne laser scanners - fork from BitBucket: https://bitbucket.org/DataspeedInc/velodyne_simulatorhttps://github.com/lmark1/velodyne_simulator(3) 下载三维点云转二维文件至工作空间的src中,具体使用教程查看我的这篇博客,教程内容无需更改即适用于本篇博客:VLP16:使用pointcloud_to_laserscan将三维点云转化为二维LaserScan_NEUer_ljx的博客-CSDN博客https://blog.csdn.net/NEUer_ljx/article/details/125741315
二、修改小车的部分文件
1.修改xacro文件
(1)修改tianbot_mini_run.urdf.xacro :根据自己情况选择小车需要的部件,我只加入了小车本体、激光雷达和imu,另外,将所引用的所有xacro文件复制到小车的urdf文件夹里,以防出错。
代码如下:
<?xml version="1.0"?>
<robot name="tianbot_mini" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--小车的xacro文件 -->
<xacro:include filename="$(find tianbot_mini)/urdf/tianbot_big.urdf.xacro" />
<!--HDL-32E雷达的xacro文件 -->
<xacro:include filename="$(find tianbot_mini)/urdf/HDL-32E.urdf.xacro"/>
<xacro:HDL-32E parent="base_link" name="velodyne" topic="/velodyne_points" organize_cloud="false" hz="10" samples="1024" gpu="false">
<origin xyz="0.3 0 1.0" rpy="0 0 0" />
</xacro:HDL-32E>
<!--imu的xacro文件 -->
<xacro:include filename="$(find tianbot_mini)/urdf/imu.xacro" />
<tianbot_mini/>
</robot>
须注意的是更改雷达的name="velodyne" topic="/velodyne_points",和后续其他文件的雷达topic要对应起来。
(2)修改小车本体的xacro文件,主要是修改gazebo插件部分:
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="base_link"/>
... ... 此处代码省略 ... ...
<!-- controller -->
<gazebo>
<plugin name="differential_drive_controller"
filename="libgazebo_ros_diff_drive.so">
<rosDebugLevel>Error</rosDebugLevel>
<publishWheelTF>false</publishWheelTF>
<robotNamespace>/</robotNamespace>
<publishTf>true</publishTf>
<publishOdomTF>false</publishOdomTF>
<publishOdom>false</publishOdom>
<publishWheelJointState>false</publishWheelJointState>
<updateRate>50</updateRate>
<legacyMode>false</legacyMode>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<wheelSeparation>0.1</wheelSeparation>
<wheelDiameter>0.043</wheelDiameter>
<broadcastTF>0</broadcastTF>
<wheelTorque>20</wheelTorque>
<wheelAcceleration>1.0</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic>
<!--<odometryFrame>tianbot_mini/odom</odometryFrame>
<odometryTopic>tianbot_mini/odom</odometryTopic>
<odometrySource>world</odometrySource> -->
<robotBaseFrame>base_link</robotBaseFrame>
</plugin>
</gazebo>
</robot>
我不需要里程计,所以发布odom有关的我都设为false,发布odom话题的我都注释掉了,根据自己情况选择。<commandTopic>cmd_vel</commandTopic> 此处跟cartographer所发布的名称相同,为cmd_vel。
(3)另附上imu.xacro文件的代码:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="lidar">
<!-- add imu llink -->
<gazebo reference="imu_link">
<material>Gazebo/Red</material>
</gazebo>
<link name="imu_link">
<visual>
<geometry>
<box size="0.05 0.05 0.05"/> <!-- 简单定义Imu形状为一个方形 -->
</geometry>
<material
name="">
<color
rgba="0.8 0.0 0.0 1.0" />
</material>
</visual>
</link>
<joint name ="imu_joints" type="fixed">
<origin xyz="0.5 0.0 0.15" rpy="0.0 0.0 0.0"/> <!-- 相对 base_link 的位置 -->
<parent link="base_link"/>
<child link="imu_link"/>
</joint>
<!-- add imu link simlations plugines -->
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>false</visualize>
<topic>/imu/data</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>/imu</topicName> <!-- 由建图算法定的名字-->
<bodyName>imu_link</bodyName>
<updateRateHZ>10.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
<initialOrientationAsReference>false</initialOrientationAsReference>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
</robot>
这里要注意 <topicName>/imu</topicName>,此处和cartographer订阅的话题名要相同,为/imu,同理,若使用其他算法,应改为该算法所订阅的名称。
2.修改launch文件
(1)simulation.launch :
代码如下:
<launch>
<arg name="robot_name" default="tianbot_mini" />
<!-- 设置launch文件的参数 -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="world_name" default="$(find tianbot_mini)/worlds/bigplane.world" /> <!-- 此处改为自己的world文件路径 -->
<!-- 运行gazebo仿真环境 -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
<remap from="tf" to="gazebo_tf"/>
</include>
<!-- 加载机器人模型描述参数 -->
<group ns="$(arg robot_name)">
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find tianbot_mini)/urdf/tianbot_mini_run.urdf.xacro' " />
<!-- 运行joint_state_publisher节点,发布机器人的关节状态 -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" >
<param name="rate" value="50"/>
</node>
<!-- 运行robot_state_publisher节点,发布tf -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<!-- 在gazebo中加载机器人模型-->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model tianbot_mini -param robot_description -model mobile_base -z 0.221320"/>
<!-- 根据需要选择是否启动RViz -->
<!--<arg name="rviz" default="true"/>
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="$(anon rviz)" respawn="false" output="screen" args="-d $(find tianbot_mini)/rviz/test3d.rviz" /> -->
</group>
</launch>
(2)修改teleop.launch
要把 ns="$(arg robot_name)" 删除,否则发布的cmd_vel话题前面会加上机器人的name,导致和gazebo订阅的/cme_vel不一样,从而无法用键盘控制小车跑。没有的忽略即可。
3、小结
到这里仿真环境就搭完了,我们启动launch文件,可以查看环境,并用键盘控制小车了。
三、创建cartographer2D建图文件
可以在cartographer的工作空间下创建文件,也可以在小车的工作空间下创建,两种方式我都试了,方法均放在这里,根据需要选择。
推荐在小车工作空间创建,避免启动时找不到文件的情况发生!!!
(一)在cartographer的工作空间下创建文件
1、新建hdl32.lua
在/carto_ws/src/cartographer_ros/cartographer_ros/configuration_files下新建hdl32.lua文件
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "imu_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true, --true
publish_frame_projected_to_2d = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 2.0, --0.2
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.min_range = 0
TRAJECTORY_BUILDER_2D.max_range = 100
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.0
TRAJECTORY_BUILDER_2D.num_accumulated_range_data=1
TRAJECTORY_BUILDER_2D.voxel_filter_size=0.025
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 1e-1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window=0.1
TRAJECTORY_BUILDER_2D.submaps.num_range_data=90 --35
POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.75
POSE_GRAPH.optimize_every_n_nodes = 90 --35
POSE_GRAPH.optimization_problem.huber_scale=10 --10
return options
2、新建hdl32_2d.launch
在/carto_ws/src/cartographer_ros/cartographer_ros/launch下新建hdl32_2d.launch文件
<launch>
<param name="/use_sim_time" value="true" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename hdl32.lua"
output="screen">
<remap from="scan" to="/scan" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>
(1) -configuration_basename hdl32.lua这里的文件名就是上一步新建的lua文件
(2)demo_2d.rviz后续可以改为自己的rviz配置文件
3、编译carto工作空间
注意编译cartographer的工作空间不能用catkin_make,要用catkin_make_isolated
cd ~/carto_ws
catkin_make_isolated --install --use-ninja
source ~/carto_ws/devel_isolated/setup.bash
一般编译完成后在/carto_ws/install_isolated/share/cartographer_ros/configuration_files 和 /carto_ws/install_isolated/share/cartographer_ros/launch 下会生成和1、2步同名的文件,如果没有就把1、2步新建的文件复制粘贴到对应路径,然后再次编译。
(二) 在小车的工作空间下创建文件
在小车工作空间的src中创建carto_nav功能包,如图所示:
1、新建hdl32.lua
在~/tianbot_mini_ws/src/carto_nav/param中新建hdl32.lua文件,代码同上。
2、新建hdl32_2d.launch
在~/tianbot_mini_ws/src/carto_nav/launch中新建hdl32_2d.launch文件
<launch>
<param name="/use_sim_time" value="true" />
<param name="robot_description" textfile="$(find tianbot_mini)/urdf/tianbot_hdl32.urdf"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find carto_nav)/param
-configuration_basename hdl32.lua"
output="screen">
<remap from="scan" to="/scan" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find carto_nav)/rviz/new_2d.rviz" />
</launch>
3、新建map_writer.sh
在~/tianbot_mini_ws/src/carto_nav/launch中新建map_writer.sh文件,用于保存pbstream地图,并将pbstream地图转换为pgm地图+地图的yaml文件。注意更改地图的存储路径及地图名称!
#!/bin/bash
rosservice call /finish_trajectory 0
rosservice call /write_state "filename: '/home/sim2real/tianbot_mini_ws/src/carto_nav/maps/maptest1.pbstream'"
rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=/home/sim2real/tianbot_mini_ws/src/carto_nav/maps/maptest1 -pbstream_filename=/home/sim2real/tianbot_mini_ws/src/carto_nav/maps/maptest1.pbstream -resolution=0.05
4、编译工作空间
cd ~/tianbot_mini_ws
catkin_make
source ~/tianbot_mini_ws/devel/setup.bash
因为要编译cartographer的文件,所以这里编译暂不清楚用 catkin_make_isolated --install --use-ninja还是普通的catkin_make,我用过isolated编译后会报各种错误,所以还是用了catkin_make。
四、建图
1、启动仿真环境
cd ~/tianbot_mini_ws/src/tianbot_mini/launch
roslaunch simulation.launch
2、启动雷达转换节点
cd ~/tianbot_mini_ws/src/pointcloud_to_laserscan-lunar-devel/launch
roslaunch my_node.launch
3、启动建图
cd ~/tianbot_mini_ws/src/carto_nav/launch
roslaunch hdl32_2d.launch
4、启动键盘控制节点
roslaunch teleop.launch
建图示例:
用画图软件打开的pgm地图:
5、保存地图
cd ~/tianbot_mini_ws/src/carto_nav/launch
./map_writer.sh
6、修图
使用画图软件对地图进行调整, 用到的软件为:kolourpaint,用法与windows画图基本相同。
安装:sudo apt install kolourpaint
在地图存储路径下:
cd ~/tianbot_mini_ws/src/carto_nav/maps
kolourpaint maptest1.pgm
调整后的地图:
总结
各个文件中的topic_name和frame_id要一致,多使用rqt_graph查看话题节点情况,使用rosrun rqt_tf_tree rqt_tf_tree查看tf_tree,使用rostopic echo /velodyne_points | grep frame_id查看frame id。