Cartographer激光2D建图定位

 1<launch>
 2  <param name="/use_sim_time" value="true" />
 3
 4  <include file="$(find cartographer_ros)/launch/ugv_2d.launch" />
 5
 6  <node name="rviz" pkg="rviz" type="rviz" required="true"
 7      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
 8  <node name="playbag" pkg="rosbag" type="play"
 9      args="--clock $(arg bag_filename)" />
10</launch>

 1<launch>
 2  <param name="robot_description"
 3    textfile="$(find cartographer_ros)/urdf/rslidar_2d.urdf" />
 4
 5  <node name="robot_state_publisher" pkg="robot_state_publisher"
 6    type="robot_state_publisher" />
 7
 8  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
 9        <remap from="cloud_in" to="/rs16_points"/>
10        <rosparam>
11            #target_frame: camera_link # Leave disabled to output scan in pointcloud frame
12            transform_tolerance: 0.01
13            min_height: 0.2     # 滤掉地面的线条
14            max_height: 1.0
15
16            angle_min: -3.1415926 # -M_PI
17            angle_max: 3.1415926 # M_PI
18            angle_increment: 0.003 # 0.17degree
19            scan_time: 0.1
20            range_min: 0.2
21            range_max: 100
22            use_inf: true
23
24            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
25            # 0 : Detect number of cores
26            # 1 : Single threaded
27            # 2->inf : Parallelism level
28            concurrency_level: 1
29        </rosparam>
30    </node>
31
32  <node name="cartographer_node" pkg="cartographer_ros"
33      type="cartographer_node" args="
34          -configuration_directory $(find cartographer_ros)/configuration_files
35          -configuration_basename ugv_2d.lua
36          -save_state_filename $(find cartographer_ros)/1.bag.pbstream"
37      output="screen">
38    <remap from="echoes" to="scan" />
39  </node>
40
41  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
42      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
43</launch>

 1<robot name="rslidar_2d">
 2  <material name="orange">
 3    <color rgba="1.0 0.5 0.2 1" />
 4  </material>
 5  <material name="gray">
 6    <color rgba="0.2 0.2 0.2 1" />
 7  </material>
 8
 9  <link name="rs16">
10    <visual>
11      <origin xyz="0 0 0" />
12      <geometry>
13        <cylinder length="0.05" radius="0.03" />
14      </geometry>
15      <material name="gray" />
16    </visual>
17  </link>
18
19  <link name="base_link" />
20
21  <joint name="rs16_joint" type="fixed">
22    <parent link="base_link" />
23    <child link="rs16" />
24    <origin xyz="0 0 0" />
25  </joint>
26
27</robot>

 1include "map_builder.lua"
 2include "trajectory_builder.lua"
 3
 4options = {
 5  map_builder = MAP_BUILDER,
 6  trajectory_builder = TRAJECTORY_BUILDER,
 7  map_frame = "map",
 8  tracking_frame = "base_link",
 9  published_frame = "base_link",
10  odom_frame = "odom",
11  provide_odom_frame = true,
12  publish_frame_projected_to_2d = false,
13  use_pose_extrapolator = true,
14  use_odometry = false,
15  use_nav_sat = false,
16  use_landmarks = false,
17  num_laser_scans = 1,
18  num_multi_echo_laser_scans = 0,
19  num_subdivisions_per_laser_scan = 1,
20  num_point_clouds = 0,
21  lookup_transform_timeout_sec = 0.2,
22  submap_publish_period_sec = 0.3,
23  pose_publish_period_sec = 5e-3,
24  trajectory_publish_period_sec = 30e-3,
25  rangefinder_sampling_ratio = 1.,
26  odometry_sampling_ratio = 1.,
27  fixed_frame_pose_sampling_ratio = 1.,
28  imu_sampling_ratio = 1,
29  landmarks_sampling_ratio = 1.,
30}
31
32MAP_BUILDER.use_trajectory_builder_2d = true
33TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
34TRAJECTORY_BUILDER_2D.use_imu_data = false
35TRAJECTORY_BUILDER_2D.min_range = 0.2
36TRAJECTORY_BUILDER_2D.max_range = 30.
37TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
38TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
39TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
40TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
41
42POSE_GRAPH.optimize_every_n_nodes = 5
43POSE_GRAPH.optimization_problem.huber_scale = 5e2
44POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
45POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 20
46POSE_GRAPH.constraint_builder.min_score = 0.5
47POSE_GRAPH.constraint_builder.global_localization_min_score = 0.55
48POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e3
49POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e3
50
51return options

1roslaunch cartographer_ros demo_ugv_2d.launch bag_filename:=/home/xxx/xxx/xxxx.bag

 1<launch>
 2  <param name="/use_sim_time" value="true" />
 3
 4  <param name="robot_description"
 5    textfile="$(find cartographer_ros)/urdf/rslidar_2d.urdf" />
 6
 7  <node name="robot_state_publisher" pkg="robot_state_publisher"
 8    type="robot_state_publisher" />
 9
10    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
11        <remap from="cloud_in" to="/rs16_points"/>
12        <rosparam>
13            #target_frame: camera_link # Leave disabled to output scan in pointcloud frame
14            transform_tolerance: 0.01
15            min_height: 0.2
16            max_height: 1.0
17
18            angle_min: -3.1415926 # -M_PI
19            angle_max: 3.1415926 # M_PI
20            angle_increment: 0.003 # 0.17degree
21            scan_time: 0.1
22            range_min: 0.2
23            range_max: 100
24            use_inf: true
25
26            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
27            # 0 : Detect number of cores
28            # 1 : Single threaded
29            # 2->inf : Parallelism level
30            concurrency_level: 1
31        </rosparam>
32    </node>
33
34  <node name="cartographer_node" pkg="cartographer_ros"
35      type="cartographer_node" args="
36          -configuration_directory $(find cartographer_ros)/configuration_files
37          -configuration_basename ugv_2d_loc.lua
38          -load_state_filename $(find cartographer_ros)/1.bag.pbstream"
39      output="screen">
40    <remap from="echoes" to="scan" />
41  </node>
42
43  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
44      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
45
46  <node name="rviz" pkg="rviz" type="rviz" required="true"
47      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
48  <node name="playbag" pkg="rosbag" type="play"
49      args="--clock $(arg bag_filename)" />
50</launch>

1include "ugv_2d.lua"
2TRAJECTORY_BUILDER.pure_localization_trimmer = {
3  max_submaps_to_keep = 5,
4}
5POSE_GRAPH.optimize_every_n_nodes = 10
6
7return options

1roslaunch cartographer_ros demo_ugv_2d_loc.launch bag_filename:=/home/xxx/xxx/xxxx.bag