前言:经过实际使用,发现cartographer定位对于重定位的表现比amcl好。如果机器人的起始位置离原点较远,amcl很难自行重定位到正确的位置,只能手动修改初始位置。然而cartographer定位的表现则很好,在行驶一段距离或原地旋转一段时间后能够正确重定位。
一、保存cartographer建的地图
1、在建图完毕后,使用cartographer自带的命令保存地图,此处可以修改存图的目录
rosservice call /write_state "filename: '${HOME}/map.pbstream' "
2、将pbstream格式的地图转换为pgm和yaml格式,以备后用。也可以使用map_server保存地图
rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=${HOME}/mymap -pbstream_filename=${HOME}/map.pbstream -resolution=0.05
二、cartographer定位的配置文件
配置3个文件,以下文件名是自定义的,可以直接在相应的目录新建一个文件,把内容复制进去
1、carto_localization.launch文件
其中scan和imu_data需要替换为自己的话题名字,map.pbstream文件的位置也要修改为自己的
<launch>
<param name="/use_sim_time" value="false" />
<!--start the graph node-->
<node name="cartographer_node_2" pkg="cartographer_ros" type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename pure_localization.lua
-load_state_filename /home/robuster/navigation_ws/src/nav_manage/map/map.pbstream"
output="screen">
<remap from="scan" to="scan" />
<remap from="points2" to="rslidar_points" />
<remap from="imu" to="imu_data" />
</node>
</launch>
2、pure_localization.lua文件
include "carto_localization.lua"
TRAJECTORY_BUILDER.pure_localization_trimmer = {
max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 80 --每80个有效帧组成一个子图,子图构建完成要闭环检测一次,这个数越小,闭环检测越频繁
return options
3、carto_localization.lua文件
注意:此处的provide_odom_frame参数会影响tf树中的odom节点。如果你机器人上的里程计精度不高,就可以将此参数置为true,也就是使用cartographer的激光里程计。这种情况下切记要关闭原本的odom -> base_link的tf转换,否则会有tf树冲突。
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER, -- map_builder.lua的配置信息
trajectory_builder = TRAJECTORY_BUILDER, -- trajectory_builder.lua的配置信息
map_frame = "map", -- 地图坐标系的名字
tracking_frame = "imu_link", -- 此处需要是imu的坐标系,否则会报错
published_frame = "base_link", -- tf: map -> base_link
odom_frame = "odom_link", -- 里程计的坐标系名字
provide_odom_frame = true, -- 是否提供odom的tf, 如果为true,则tf树为map->odom->base_link
-- 如果为false tf树为map->base_link
publish_frame_projected_to_2d = false, -- 是否将坐标系投影到平面上
--use_pose_extrapolator = false, -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿
use_odometry = false, -- 是否使用里程计,如果使用要求一定要有odom的tf
use_nav_sat = false, -- 是否使用gps
use_landmarks = false, -- 是否使用landmark
num_laser_scans = 0, -- 是否使用单线激光数据
num_multi_echo_laser_scans = 0, -- 是否使用multi_echo_laser_scans数据
num_subdivisions_per_laser_scan = 1, -- 1帧数据被分成几次处理,一般为1
num_point_clouds = 1, -- 是否使用点云数据
lookup_transform_timeout_sec = 0.2, -- 查找tf时的超时时间
submap_publish_period_sec = 0.3, -- 发布数据的时间间隔
pose_publish_period_sec = 0.1, -- 发布tf坐标转换的频率
trajectory_publish_period_sec = 0.1,
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.use_imu_data = true
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.min_z = 0.2
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1
POSE_GRAPH.optimize_every_n_nodes = 160.
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.min_score = 0.48
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60
return options
三、使用cartographer定位 发布实时的全局位置信息
1、找到cartographer_ros包中的node.cc文件,查找里面的publish_tracked_pose,可以看到这个变量决定是否发布定位信息。转到publish_tracked_pose的定义处,将其修改为true
bool publish_tracked_pose = true;
2、运行上面的carto_localization.launch文件,查询所有话题可以发现,出现了一个tracked_pose话题,这个就是cartographer节点发布的全局定位信息。
3、 用cartographer替换amcl的定位
amcl_pose的数据类型是PoseWithCovarianceStamped,tracked_pose的数据类型是PoseStamped,这两个数据类型都有x,y,z和四元数的信息。因此可以直接将订阅amcl_pose的代码替换为订阅tracked_pose。
订阅amcl_pose的代码
void DWAPlanner::amclCallBack(const geometry_msgs::PoseWithCovarianceStamped& amcl_msg)
{
double cur_pose_x = amcl_msg.pose.pose.position.x;
double cur_pose_y = amcl_msg.pose.pose.position.y;
double x = amcl_msg.pose.pose.orientation.x;
double y = amcl_msg.pose.pose.orientation.y;
double z = amcl_msg.pose.pose.orientation.z;
double w = amcl_msg.pose.pose.orientation.w;
double siny_cosp = 2 * (w * z + x * y);
double cosy_cosp = 1 - 2 * (y * y + z * z);
double cur_pose_theta = std::atan2(siny_cosp, cosy_cosp); // rad/s
cur_pose_[0] = cur_pose_x;
cur_pose_[1] = cur_pose_y;
cur_pose_[2] = cur_pose_theta;
}
替换为订阅tracked_pose
void DWAPlanner::cartoCallBack(const geometry_msgs::PoseStamped& carto_msg)
{
double cur_pose_x = carto_msg.pose.position.x;
double cur_pose_y = carto_msg.pose.position.y;
double x = carto_msg.pose.orientation.x;
double y = carto_msg.pose.orientation.y;
double z = carto_msg.pose.orientation.z;
double w = carto_msg.pose.orientation.w;
double siny_cosp = 2 * (w * z + x * y);
double cosy_cosp = 1 - 2 * (y * y + z * z);
double cur_pose_theta = std::atan2(siny_cosp, cosy_cosp); // rad/s
cur_pose_[0] = cur_pose_x;
cur_pose_[1] = cur_pose_y;
cur_pose_[2] = cur_pose_theta;
}