cartographer_occupancy_grid_node provide a bad map for ros-navigation #864

Posted 2020-11-05 hiram-zhang

hi!
**1.**I use cartographer_node to build 2D map,and use cartographer_occupancy_grid_node to provide a ros map.My configuration file is as follows
**[filename: xiaobao_mapbuild.launch]**
`<launch>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename xiaobao_mapbuild.lua"
output="screen">
<remap from="scan" to="/scan" />
<remap from="imu" to="/imu" />
<remap from="odom" to="/odom" />
</node>

<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>`
**[filename: xiaobao_mapbuild.lua]**
`include "map_builder.lua"
include "trajectory_builder.lua"

options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "imu_link", --default:"base_link","imu_link" if use imu
published_frame = "base_footprint", --default:"base_link"
odom_frame = "odom_carto", --default:"odom",no-use if provide_odom_frame = false
provide_odom_frame = false, --default:true
publish_frame_projected_to_2d = false,
use_odometry = true, --default:false
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1, --default:0
num_multi_echo_laser_scans = 0, --default:1
num_subdivisions_per_laser_scan = 10,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2, --default: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
--new params add
TRAJECTORY_BUILDER_2D.min_range = 0.15 --lidar:LS01D
TRAJECTORY_BUILDER_2D.max_range = 8.0 --lidar:LS01D
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 90
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 10.6
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 10
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40
POSE_GRAPH.constraint_builder.max_constraint_distance = 4.

return options`
when I bringup my robot xiaobao,cartographer_node and cartographer_occupancy_grid_node can work well. Meanwhile，I can see a gird map in rviz.

**2.**Then, I want to use cartographer and ros-navigation to building map with exploration. So
I bringup ros-navigation, My configuration file is as follows
**[filename: xiaobao_nav.launch]**
`<launch>
<!--- Run AMCL -->
<arg name="initial_pose_x" default="-1.13"/> <!-- Use 17.0 for willow‘s map in simulation -->
<arg name="initial_pose_y" default="3.87"/> <!-- Use 17.0 for willow‘s map in simulation -->
<arg name="initial_pose_a" default="3.14"/>
<include file="$(find xiaobao_nav)/config/amcl.launch.xml">
<arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
<arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
<arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
</include>

<!-- Run move_base -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<rosparam file="$(find xiaobao_nav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find xiaobao_nav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find xiaobao_nav)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find xiaobao_nav)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find xiaobao_nav)/config/move_base_params.yaml" command="load" />
<rosparam file="$(find xiaobao_nav)/config/base_local_planner_params.yaml" command="load" />
<rosparam file="$(find xiaobao_nav)/config/global_planner_params.yaml" command="load" />
</node>

</launch>`
**[filename: amcl.launch.xml]**
`<launch>
<!-- amcl params for dashgo_base of EAI-E1 -->
<arg name="use_map_topic" default="true"/>
<arg name="scan_topic" default="scan"/>
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<arg name="odom_frame_id" default="odom"/>
<!--arg name="odom_frame_id" default="laser_odom"/-->
<arg name="base_frame_id" default="base_footprint"/>
<arg name="global_frame_id" default="map"/>

<node pkg="amcl" type="amcl" name="amcl">
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="16.0"/>
<param name="min_particles" value="2000"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
<param name="resample_interval" value="0.5"/>
<!-- Increase tolerance because the computer can get quite busy -->
<param name="transform_tolerance" value="1.0"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
`
move_base node and amcl node of ros-navigation can work well too. **But problem is Particle point of AMCL Never converge ，no matter how I move the robot in grid map(ps:I have give a good initial pose by 2D_Pose_Estimae button of rviz).**

**3.**On the one hand，I use ‘run map_server map_saver -f publisher_map‘ save ros map as publisher_map.pgm and publisher_map.yaml,when cartographer_node running.
On the other hand， I save submapList into *.pbstream by call rosservice /write_state.After,I use cartographer_pbstream_to_ros_map to save *.pbstream into ros_map.pgm and ros_map.yaml.
**But provide ros_map.pgm by ‘run map_server map_server ros_map.yaml‘,AMCL can work well.**

**4.**when I Compare two pictures，I found publisher_map.pgm is much less than ros_map.pgm texture.So I go to read source code occupancy_grid_node_main.cc and pbstream_to_ros_map_main.cc, **I seem to understand when I read line 132 in occupancy_grid_node_main.cc. cartographer_occupancy_grid_node just publish an incomplete map，why notpublish an complete map.**