hector slam+gmapping+odometry problem.

edit

Hello all,

I am trying to implement autonomous mobile robot using ydlidar and encoder odometry data which is pretty much same as what turtlebot 3 is doing. All the codes are available for download from git https://github.com/omorobot/omoros

FYI, I am running this code on a Jetson TX2 with latest L4T with ROS-melodic. All the computation and sensor, driver interface is done in a stand alone system.

Launch directory contains driver relate files as below. When this file is called, robot platform is up and running with joystick input and shows robot base_link is moving per actual robot.

<launch>
   <arg name="multi_robot_name" default=""/>
   <include file="$(find omoros)/launch/includes/description.launch.xml">
   </include>
   <node respawn="true" pkg="joy" type="joy_node" name="teleop_joy"/>
   <node pkg="omoros" type="driver_r1.py" name="omoros" output="screen">
      <param name="port" value="/dev/ttyMotor"/>
      <param name="baud" value="115200"/>
      <param name="modelName" value="r1"/>
   </node>
   <node pkg="tf" type="static_transform_publisher" name="map_to_odom" 
         args="0.0 0.0 0.0 0 0 0.0 /map /odom 40" />
   <node pkg="tf" type="static_transform_publisher" name="odom_to_basefootprint" 
         args="0.0 0.0 0.0 0.0 0.0 0.0 /odom /base_footprint 40" />
   <node pkg="tf" type="static_transform_publisher" name="base_link_to_wheel_left_link" 
         args="0.0  0.29 0.11 0.0 0.0 0.0 /base_link /wheel_left_link 40" />
   <node pkg="tf" type="static_transform_publisher" name="base_link_to_wheel_right_link" 
         args="0.0 -0.29 0.11 0.0 0.0 0.0 /base_link /wheel_right_link 40" />

   <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" value="$(arg multi_robot_name)"/>
   </node>
</launch>

And problem starts when tries to run navigation launch file below for auto navigation as seen in this short video. https://youtu.be/T4GuwDxQe7g

It can start generate map but is moves back and forth and is not stable for driving. When tries to set goal, the robot starts rotating in position and never start moving.

<launch>
  <arg name="map_file" default="$(find omoros)/navigation/maps/map.yaml"/>
  <arg name="open_rviz" default="true"/>
  <arg name="move_forward_only" default="false"/>

  <!-- Launch hector slam with ydlidar -->    
  <include file="$(find omoros)/launch/lidar.launch" />   
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" 
        args="0.0 0.0 0.2 0.0 0.0 0.0 /base_link /laser_frame 40" />        
  <include file="$(find hector_mapping)/launch/mapping_default.launch" />
  <!-- Map server -->
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
   <!-- AMCL -->
  <include file="$(find omoros)/launch/amcl.launch"/>
  <!-- move_base -->
  <include file="$(find omoros)/launch/move_base.launch">
    <arg name="move_forward_only" value="$(arg move_forward_only)"/> </include>
   <!-- rviz -->
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find omoros)/navigation/rviz/omoros_navigation.rviz" />
   <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch" />
</launch>

my amcl.launch is as below

<launch>
  <!-- Arguments -->
  <arg name="scan_topic"     default="scan ...