在前面已经完成了cartographer_ros的安装和demo的运行了。接下来,就要放到机器人上,实时进行SLAM了。
前一篇内容的链接如下:
Cartographer_ros的下载、配置及编译与问题处理_hxlanu的博客-CSDN博客_ros 编译
开始时基本的思路就是修改demo的文件。当时用的就是offline_backpack_2d.launch这个文件。这个能做到,但是要修改的很多,下面就来解决。(更推荐基于demo_revo_lds.launch这个文件进行修改,这个可以看后面问题部分。)
cartographer_ros的官方教程里提供了录制的二维激光雷达和IMU的bag包,如果像替换成其他雷达的数据,还需要做一些修改。
cartographer_ros提供的二维激光雷达数据包发布的数据topic包括/clock, /horizontal_laser_2d, /vertical_laser_2d和/imu。而二维激光雷达发布的激光扫描数据一般为/scan。另外,/horizontal_laser_2d和/vertical_laser_2d两个topic的数据类型都是sensor_msgs::MultiEchoLaserScan,而一般的雷达数据都是sensor_msgs::LaserScan类型,因此如果要使用二维激光雷达,要么进行数据类型转换后转发,要么更改cartographer_ros的设置一边接收sensor_msgs::laserscan类型。
数据类型转换法
先看sensor_msgs::MultiEchoLaserScan的数据
# Single scan from a multi-echo planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data
Header header # timestamp in the header is the acquisition time of
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad]
float32 time_increment # time between measurements [seconds] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
float32 scan_time # time between scans [seconds]
float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m]
LaserEcho[] ranges # range data [m] (Note: NaNs, values < range_min or > range_max should be discarded)
# +Inf measurements are out of range
# -Inf measurements are too close to determine exact distance.
LaserEcho[] intensities # intensity data [device-specific units]. If your
# device does not provide intensities, please leave
# the array empty.
而sensor_msgs::LaserScan的数据则是
# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data
Header header # timestamp in the header is the acquisition time of
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
float32 angle_min # start angle of the scan [rad]
float32 angle_max # end angle of the scan [rad]
float32 angle_increment # angular distance between measurements [rad]
float32 time_increment # time between measurements [seconds] - if your scanner
# is moving, this will be used in interpolating position
# of 3d points
float32 scan_time # time between scans [seconds]
float32 range_min # minimum range value [m]
float32 range_max # maximum range value [m]
float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities # intensity data [device-specific units]. If your
# device does not provide intensities, please leave
# the array empty.
对比之后可以发现,主要区别就是ranges和intensities这两个变量的数据类型。
因此,只需在转发程序里将ranges和intensities的数据进行转换。具体转换部分如下:
float range;
float inten;
sensor_msgs::LaserEcho echo;
sensor_msgs::LaserEcho intens;
for(int i = 0; i < datalength; i++){ //datalength是激光的数据点数
range = msg->ranges[i];//msg是激光数据topic的回调函数的消息指针
inten = msg->intensities;
echo.echoes.push_back(range);
muls.ranges.push_back(echo);
intens.echoes.push_back(inten);
muls.intensities.push_back(intens);
}
注意,在数据转换的时候,消息的header和其他参数也需要相应的拷贝过去,一般直接拷贝就可以了。如果数据不全,则须按照激光雷达的参数进行设置。
Header 包含seq、stamp、frame_id,其中seq扫描顺序,stamp是时间戳,frame_id是雷达的名字。
其他参数的含义:
angle_min:第一个激光数据的角度
angle_max:最后一个激光数据的角度
angle_increment:相邻两个激光数据的角度差
time_increment: 测量时间间隔
scan_time :扫描时间间隔
range_min:测距最小值
range_max:测距最大值
最后,转发的topic叫/horizontal_laser_2d
参数设置法
参数设置法主要通过在cartographer_ros的lua中进行设置。demo里的offline_backpack_2d.launch文件里加载的backpack_2d.lua中有这样两行代码:
num_laser_scans = 0,
num_multi_echo_laser_scans = 1,
这里实际可通过更改这两个参数的数字来设置cartographer_ros接收哪种数据。如果num_laser_scans为1则默认接收sensor_msgs::LaserScan数据的topic;如果num_multi_echo_laser_scans为1则默认接收sensor_msgs::MultiEchoLaserScan数据的topic。
如果采用的通过修改demo_revo_lds.launch和revo_lds.lua文件,则默认是使用sensor_msgs::LaserScan的topic数据类型。
遇到的问题
用前面的方法解决数据匹配后,如果直接运行,会发现还会遇到很多问题,接下来将一个一个的解决。
问题一
由于不再需要录制的数据包,所以对launch文件进行修改,删除bag部分,修改后的offline_backpack_2d.launch为:
<launch>
<arg name="no_rviz" default="false"/>
<arg name="rviz_config" default="$(find cartographer_ros)/configuration_files/demo_2d.rviz"/>
<arg name="configuration_directory" default="$(find cartographer_ros)/configuration_files"/>
<arg name="configuration_basenames" default="backpack_2d.lua"/>
<arg name="urdf_filenames" default="$(find cartographer_ros)/urdf/backpack_2d.urdf"/>
<arg name="launch_prefix" default=""/>
<remap from="echoes" to="horizontal_laser_2d"/>
<include file="$(find cartographer_ros)/launch/offline_node.launch">
<arg name="bag_filenames" value="$(arg bag_filenames)"/>
<arg name="no_rviz" value="$(arg no_rviz)"/>
<arg name="rviz_config" value="$(arg rviz_config)"/>
<arg name="configuration_directory" value="$(arg configuration_directory)"/>
<arg name="configuration_basenames" value="$(arg configuration_basenames)"/>
<arg name="urdf_filenames" value="$(arg urdf_filenames)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
</include>
</launch>
注意,下面的这个语句
<remap from="echoes" to="horizontal_laser_2d"/>
如果仍然采用sensor_msgs::MultiEchoLaserScan这个数据类型的话,保持不变;如果改用sensor_msgs::LaserScan,则需要把echoes改为scan。
问题二
如果通过前面的方法直接用demo中的launch文件进行运行的话,就会遇到缺少/imu这个topic的问题,解决方法就是修改lua文件。打开launch文件中对应的lua文件,找到num_subdivisions_per_laser_scan这个参数,并做如下修改:
首先找到
MAP_BUILDER.use_trajectory_builder_2d = true
删除后面的
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
然后参考revo_lds.lua,添加如下部分:
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
修改后即可解决上述问题。
问题三
如果通过前面的方法直接用demo中的launch文件进行运行的话,就会遇到一下问题:
Ignored subdivision of a LaserScan message from sensor scan because previous subdivision time xxxxxxxxx is not before current subdivision time xxxxxxxxx
解决这个问题的办法还是修改lua文件。打开launch文件中对应的lua文件,找到num_subdivisions_per_laser_scan这个参数,并做如下修改:
num_subdivisions_per_laser_scan = 1,
同时,在launch文件中,还需要添加如下语句:
<param name="use_sim_time" value="false" />
通过以上修改就可以解决前面的问题了。
问题四
如果运行过程中出现以下提示:
range_data_collator.cc:82] Dropped 1 earlier points.
这是由于激光的数据问题。可以通过在转发的时候给header.stamp赋予现在的时间即可。
sensor_msgs::LaserScan ls.header.stamp = ros::Time::now();
问题五
如果需要对sensor_msgs::laserscan进行转发,可能遇到一下问题:
segmentation fault
这个问题发生在
ls.ranges[i] = msg->ranges[i];
这是因为ls这个变量定义后,它的成员ranges需要确定大小。所以解决方法就是在给它赋值前进行初始化,初始化方法如下:
ls.ranges.resize(num);
ls.intensities.resize(num);
这里num是激光雷达的数据长度。
至此,可以应该就可以运行cartographer_ros。
我主要是通过修改backpack_2d.launch文件和backpack_2d.lua文件实现。为了实现直接采用激光雷达数据,则需删除最后的rosbag的部分。修改后的launch文件如下:
<launch>
<param name="/use_sim_time" value="false" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename revo_lds.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>
修改后的lua文件为:
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
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 = 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.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 12.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
接下来就是运行了cartographer_ros了!
当然,lua里的参数还需要修改才能实现较好的效果,可以参考一下两个网址:
Cartographer_ros解读 - ColinQin - 博客园 (cnblogs.com)
Tuning methodology — Cartographer ROS documentation (google-cartographer-ros.readthedocs.io)
Configuration — Cartographer documentation (google-cartographer.readthedocs.io)
基于激光雷达定位和DWA的小型无人机穿越窄门演示