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"/>
  <arg name="initial_pose_x" default="0.0"/>
  <arg name="initial_pose_y" default="0.0"/>
  <arg name="initial_pose_a" default="0.0"/>

  <!-- AMCL -->
  <node pkg="amcl" type="amcl" name="amcl">

    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="3000"/>
    <param name="kld_err"                   value="0.02"/>
    <param name="update_min_d"              value="0.20"/>
    <param name="update_min_a"              value="0.20"/>
    <param name="resample_interval"         value="1"/>
    <param name="transform_tolerance"       value="0.5"/>
    <param name="recovery_alpha_slow"       value="0.00"/>
    <param name="recovery_alpha_fast"       value="0.00"/>
    <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)"/>
    <param name="gui_publish_rate"          value="50.0"/>

    <remap from="scan"                      to="$(arg scan_topic)"/>
    <param name="laser_max_range"           value="3.5"/>
    <param name="laser_max_beams"           value="180"/>
    <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_likelihood_max_dist" value="2.0"/>
    <param name="laser_model_type"          value="likelihood_field"/>

    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha1"               value="0.1"/>
    <param name="odom_alpha2"               value="0.1"/>
    <param name="odom_alpha3"               value="0.1"/>
    <param name="odom_alpha4"               value="0.1"/>
    <param name="odom_frame_id"             value="odom"/>
   <param name="base_frame_id"             value="base_link"/>
  </node>
</launch>

Movebase launch as below

<launch>
  <!-- Arguments -->
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="move_forward_only" default="false"/>

  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <rosparam file="$(find omoros)/navigation/param/costmap_common_params_r1.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find omoros)/navigation/param/costmap_common_params_r1.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find omoros)/navigation/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find omoros)/navigation/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find omoros)/navigation/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find omoros)/navigation/param/dwa_local_planner_params_r1.yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
  </node>
</launch>

It seems like something is wrong with the above setup and not sure what it is. I tried to make it as same as what turtlebot3 does for navigation but is not as simple it may looks like. Any help would be much appreciated. Thanks,

Kyu