参考链接:https://blog.csdn.net/weixin_36976685/article/details/103834608
一、下载数据集
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag
二、建图
roslaunch cartographer_ros offline_backpack_2d_zhan.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag
三、纯定位测试
这里后面两个参数一个是上一步建好的地图,pbstream格式,另一个是数据集。也就是说,先用第一个数据集建图,建完图后用第二个数据集进行定位测试。
定位判断:看机器人在地图中的位置是否正确。经过一段时间,如果scan和map能匹配上,则定位成功。
roslaunch cartographer_ros demo_backpack_2d_localization_zhan.launch \
load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag
四、配置文件
(1)offline_backpack_2d_zhan.launch
<launch>
<arg name="bag_filenames"/>
<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_zhan.lua"/>
<arg name="urdf_filenames" default="$(find cartographer_ros)/urdf/backpack_2d_zhan.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>
(2)backpack_2d_zhan.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 = 0,
num_multi_echo_laser_scans = 1,
num_subdivisions_per_laser_scan = 10,
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.num_accumulated_range_data = 10
return options
(3)backpack_2d_zhan.urdf
<robot name="cartographer_backpack_2d">
<material name="orange">
<color rgba="1.0 0.5 0.2 1" />
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1" />
</material>
<link name="imu_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<box size="0.06 0.04 0.02" />
</geometry>
<material name="orange" />
</visual>
</link>
<link name="horizontal_laser_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.03" />
</geometry>
<material name="gray" />
</visual>
</link>
<link name="vertical_laser_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.03" />
</geometry>
<material name="gray" />
</visual>
</link>
<link name="base_link" />
<joint name="imu_link_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="0 0 0" />
</joint>
<joint name="horizontal_laser_link_joint" type="fixed">
<parent link="base_link" />
<child link="horizontal_laser_link" />
<origin xyz="0.1007 0 0.0558" />
</joint>
<joint name="vertical_laser_link_joint" type="fixed">
<parent link="base_link" />
<child link="vertical_laser_link" />
<origin rpy="0 -1.570796 3.141593" xyz="0.1007 0 0.1814" />
</joint>
</robot>
(4)demo_backpack_2d_localization_zhan.launch
<launch>
<param name="/use_sim_time" value="true" />
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d_zhan.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 cartographer_ros)/configuration_files
-configuration_basename backpack_2d_localization_zhan.lua
-load_state_filename $(arg load_state_filename)"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</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" />
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>
(5)backpack_2d_localization_zhan.lua
include "backpack_2d_zhan.lua"
TRAJECTORY_BUILDER.pure_localization_trimmer = {
max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20
return options