使用cartographer纯定位替换amcl定位

前言：经过实际使用，发现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" /><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_linkodom_frame = "odom_link",                 -- 里程计的坐标系名字provide_odom_frame = true,                -- 是否提供odom的tf, 如果为true,则tf树为map->odom->base_link-- 如果为false tf树为map->base_linkpublish_frame_projected_to_2d = false,    -- 是否将坐标系投影到平面上--use_pose_extrapolator = false,          -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿use_odometry = false,                     -- 是否使用里程计,如果使用要求一定要有odom的tfuse_nav_sat = false,                      -- 是否使用gpsuse_landmarks = false,                    -- 是否使用landmarknum_laser_scans = 0,                      -- 是否使用单线激光数据num_multi_echo_laser_scans = 0,           -- 是否使用multi_echo_laser_scans数据num_subdivisions_per_laser_scan = 1,      -- 1帧数据被分成几次处理,一般为1num_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 = trueTRAJECTORY_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.2TRAJECTORY_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.1POSE_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.60return 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/scur_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/scur_pose_[0] = cur_pose_x;cur_pose_[1] = cur_pose_y;cur_pose_[2] = cur_pose_theta;
}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！