Localization issue with hector_mapping and amcl

edit

Hi All,

I'm trying to implement autonomous navigation on a quadruped robot (Unitree A1 with RPLIDAR M2M2 Mapper) from a pre-saved map. I'm loading the map via map_server, getting odometry via hector_mapping, and localizing on the map via amcl. I carefully set the initial pose in rviz (aligning the red dots from /scan with the walls from the map), and walk the robot around with a controller. I notice that the robot is not localizing, as the particles from particlecloud/ topic from amcl get further and further away from the robot as it keeps walking. I'm not sure why amcl is not able to localize, and am interested if anyone notices anything wrong with my setup. Below I attach all relevant launch files, and the tf tree. Thanks!

guide_dog_nav.launch <launch>

<!-- Load pre-built map to topic /map -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find guide_dog_nav)/maps/segbot_generated_map.yaml"/>

<!-- Load Lidar drivers to get /scan topic -->
<include file="$(find guide_dog_nav)/launch/rplidar_start.launch" />

<node name="base2laser" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 1 base_footprint laser_frame 100"/>

<!-- messages published to scanmatch_odom frame get sent to base_footprint frame -->
<node name="odomtransformer" pkg="guide_dog_nav" type="odomtransformer.py" output="screen">
    <param name="odom_input" value="scanmatch_odom" />
    <param name="tf_output" value="base_footprint" />
</node>

<!-- Hector SLAM estimates odometry based on /scan messages -->
<!-- Inspired by: https://github.com/ne0h/hmmwv/blob/master/ros_workspace/src/hmmwv/launch/prebuildmap.launch -->

<node name="hector_mapping" pkg="hector_mapping" type="hector_mapping">

    <remap from= "map"      to= "/mapcurrent"/>
    <param name="map_frame" value="/mapcurrent" />

    <param name="base_frame" value="base_footprint" />
    <param name="odom_frame" value="scanmatch_odom" />
    <param name="pub_odometry" value="true" />
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="false" />

    <param name="advertise_map_service" value="true" />

    <param name="pub_map_scanmatch_transform" value="false" />
    <param name="tf_map_scanmatch_transform_frame_name" value="scanmatcher_frame" />

</node>

<!-- AMCL localizes robot in map frame. Adds transform from /map -> /odom frames -->
<node name="amcl" pkg="amcl" type="amcl" output="screen">

    <param name="base_frame_id" value="base_footprint" />
    <param name="global_frame_id" value="map" />
    <param name="odom_frame_id" value="scanmatch_odom" />
    <param name="odom_model_type" value="diff-corrected"/>

    <param name="use_map_topic" value="true"/>

</node>

</launch>

rplidar_start.launch

<launch>
<arg name="ip_address" default="192.168.11.1" /> 
<node name="slamware_ros_sdk_server_node" pkg="slamware_ros_sdk" type="slamware_ros_sdk_server_node" output="screen">

<param name= "ip_address"               value= "$(arg ip_address)"/>
<param name= "angle_compensate"         value= "true"/>        
<param name= "fixed_odom_map_tf"        value= "false"/>        

<param name= "robot_frame"              value= "base_footprint"/>   
<param name= "laser_frame"              value= "laser_frame"/>       

<!-- We don't consider these slamware defined frames -->
<param name= "odom_frame"               value= "slamware_odom"/>       
<param name= "map_frame"                value= "slamware_map"/>    

<remap from= "/slamware_ros_sdk_server_node/scan"      to= "scan"/>
</node>
</launch>

I'm not allowed to upload images, but my tf tree is:

map -> scanmatch_odom -> base_footprint -> laser_frame

map -> scanmatch_odom is ... (more)