First time poster. Long time user.! Hoping for some guidance. Using Jetson nano running Ubuntu 18.04 + ROS Melodic. Im Connecting to the robot using a remote laptop. Have set up the networking to allow for rostopics to pub/sub both ways between the robot and the laptop.

Ive been following multiple tutorials on how to get Navstack working on a real robot, predominantly Automatic Addisons tutorial as i thought it was well written and straight forward... (https://automaticaddison.com/how-to-set-up-the-ros-navigation-stack-on-a-robot)

I've set up my navstack, using a BNO055 + encoder motors to count ticks etc. using an arduino as a drive controller to drive the wheels based on "cmd_vel" values. ...but im stuck.

When i launch my navigation stack, everything launches as i believe its supposed to. I can set an initial pose and my AMCL gets to work and localises the robot in the map and lines the laser scan to the walls on the map good enough. My issue starts when i set a goal pose. The path shows up so my planning seems to work and my robot moves IRL but not in RVIZ which causes my robot to either

1. just go in a straight line as its being sent constant cmd_vel to reach the first pose in the path?
2. oscillates to try and correct itself trying to get to the first pose.

I am under the impression that my issues are within some transform or odom not publishing correctly, causing my TF to stay in the same spot in RVIZ. My TF frames tree looks correct: map>odom>base_footprint>base_link and my rqt_graph looks okay and matches the one on the tutorial, only difference is tutorial has TrajectoryPlannerROS node and mine is using Navfn .

When my robot moves across the apartment where im testing, the laser scan moves on rviz because its on the robot that is moving across my kitchen), but the robot TF on rviz stays at initial pose. Ive been stuck for a bit. i even hit up the original tutorial poster but he left me on read. punk a$$ 8!tch. Any help would appreciated!

This is my launch file:

<launch>

  <!-- Transformation Configuration ... Setting Up the Relationships Between Coordinate Frames --> 
  <node pkg="tf" type="static_transform_publisher" name="robot_footprint_to_base_link_bc" args="0 0 0.05 0 0 0 base_footprint base_link 30" /> <!-- Transform between the base_footprint ("Robots shadow on the ground plane") and base_link ("The robots chassis") -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser_bc" args="0.04 0 0.06 0 0 0 base_link laser 30" /> <!-- Between base_link ("The robots chassis") and laser (|The robots RPlidar") -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_imu_bc" args="0.02 0.0 0.005 0 0 0 base_link imu 30" /> <!-- Between base_link ("The robots chassis") and imu (|The robots MPU-6050/GY521") -->

  <!-- map to odom will be provided by the AMCL -->
  <!-- <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 40" /> -->

  <!-- motor start node.  -->
  <!-- <node name="bbboy_motors" pkg="bbboy_simulation" type="bbboy_motors_2.py">
  </node> -->

  <!-- Wheel Encoder Tick Publisher and Base Controller Using Arduino (motor_controller_diff_drive_2.ino)
        Subscribe:  (/cmd_vel)                      type: -
        Publish:    (/right_ticks)                  type: std_msgs/Int16
                    (/left_ticks)                   type: std_msgs/Int16                     -->
  <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="/dev/ttyUSB0"/>
    <param name="baud" value="115200"/>
  </node>

  <!--  Wheel Odometry Publisher -->
  <!--  Subscribe:  (/right_ticks)                  type: std_msgs/Int16
                    (/left_ticks)                   type: std_msgs/Int16
                    (/initial_2d)                   type: geometry_msgs/PoseStamped
        Publish:    (/odom_data_euler)              type: nav_msgs/Odometry
                    (/odom_data_quat)               type: nav_msgs/Odometry                                -->
  <node pkg="bbboy_navstack" type="ekf_odom_publisher" name="ekf_odom_publisher">
  </node> 

  <!-- IMU Data Publisher Using the BNO055 IMU Sensor -->
<!-- Publish: /imu/data -->
<node ns="imu" name="imu_node" pkg="imu_bno055" type="bno055_i2c_node" respawn="true" respawn_delay="2">
  <param name="device" type="string" value="/dev/i2c-1"/>
  <param name="address" type="int" value="40"/> <!-- 0x28 == 40 is the default for BNO055 -->
  <param name="frame_id" type="string" value="imu"/>
</node>

  <!--  Extended Kalman Filter from robot_pose_ekf Node                 -->
  <!--  Subscribe:  (/odom) > /odom_data_quat       type: nav_msgs/Odometry
                    (/imu_data) >  /imu/data        type: sensor_msgs/Imu
                    (/vo)                           type: nav_msgs/Odometry
        Publish:    (/robot_pose_ekf/odom_combined) type: geometry_msgs/PoseWithCovarianceStamped                -->
  <remap from="odom" to="odom_data_quat" />
  <remap from="imu_data" to="imu/data" />
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/> <!--odom -->
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="40.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="gps_used" value="false"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>

  <!--  Initial Pose and Goal Publisher -->
   <!-- Publish:    (/initialpose)                  type: geometry_msgs/PoseWithCovarianceStamped
                    (/move_base_simple/goal)        type: geometry_msgs/PoseStamped       --> 
  <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find bbboy_simulation)/rviz_configs/navigation.rviz">
  </node> -->

  <!--  Subscribe:  (/initialpose)                  type: geometry_msgs/PoseWithCovarianceStamped 
                    (/move_base_simple/goal)        type: geometry_msgs/PoseStamped 
        Publish:    (/initial_2d)                   type: geometry_msgs/PoseStamped
                    (/goal_2d)                      type: geometry_msgs/PoseStamped                           --> 
  <node pkg="bbboy_navstack" type="rviz_click_to_2d" name="rviz_click_to_2d">
  </node>   

  <!--  Lidar Data Publisher Using RPLIDAR from Slamtec -->
  <!--  Dont forget you may need to  $ sudo chmod 666 /dev/ttyUSB0 to give device access. -->
  <!--  Publish:    (/scan)                         type: sensor_msgs/LaserScan                         -->

  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
    <param name="serial_port"         type="string" value="/dev/ttyUSB1"/>
    <param name="serial_baudrate"     type="int"    value="115200"/>  <!--A1/A2 :: A3 = 256000 -->
    <param name="frame_id"            type="string" value="laser"/>
    <param name="inverted"            type="bool"   value="false"/>
    <param name="angle_compensate"    type="bool"   value="true"/>
  </node>  

  <!--  Map File -->
  <arg name="map_file" default="$(find bbboy_navstack)/maps/my_hector_map.yaml"/>


  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
  <param name="frame_id" value="/odom" />

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

<!-- <remap from="odom" to="robot_pose_ekf/odom_combined" /> -->
  <!-- <param name="initial_pose_x" value="-0.350" />
    <param name="initial_pose_y" value="0.263" />
    <param name="initial_pose_a" value="-1.654" />   -->
        <param name="odom_frame_id" value="odom" />
        <param name="odom_model_type" value="diff" />
        <param name="base_frame_id" value="base_footprint" />
        <param name="global_frame_id" value="map" />
        <param name="odom_alpha1" value="0.01"/>
        <param name="odom_alpha2" value="0.01"/>
        <param name="odom_alpha3" value="0.01"/>
        <param name="odom_alpha4" value="0.01"/>
        <param name="odom_alpha5" value="0.05"/>

        <!-- Filter Parameters: -->
        <param name="min_particles" value="500" />
        <param name="max_particles" value="4000" />
        <param name="kld_err" value="0.01" /> 
        <param name="kld_z" value="0.99" />
        <param name="update_min_a" value="-1.00" />
        <param name="update_min_d" value="-1.00" />
        <param name="resample_interval" value="2" />
        <param name="transform_tolerance" value="0.1" />
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>

        <!-- Additional Parameters: Laser -->
        <param name="laser_z_hit" value="0.95" />
        <param name="laser_z_rand" value="0.05" />
        <param name="laser_max_beams" value="90" />
        <!-- <param name="laser_min_range" value="0.1" />  -->

        <!-- Troubleshooting: tf error: canTransform: target_frame map does not exist -->
    <param name="tf_broadcast" value="true" />
        <param name="first_map_only" value="false" />
    </node>

  <!--  Move Base Node -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">`
    <rosparam file="$(find bbboy_navstack)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find bbboy_navstack)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find bbboy_navstack)/params/local_costmap_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find bbboy_navstack)/params/global_costmap_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find bbboy_navstack)/params/base_local_planner_params.yaml" command="load" />

  </node>

</launch>

BaseLocalplanner:

controller_frequency: 10

TrajectoryPlannerROS:
  max_vel_x: 1.0 #0.5
  min_vel_x: 0.7 #0.01
  max_vel_theta: 1.57 #1.5

  min_in_place_vel_theta: 0.314

  acc_lim_theta: 3.14
  acc_lim_x: 2.0
  acc_lim_y: 2.0

  sim_time: 1.0

  vx_samples: 5.0
  vtheta_samples: 10.0

  #pdist_scale: 0.6 #0.6
  path_distance_bias: 0.6
  #gdist_scale: 0.8 #0.8
  goal_distance_bias: 0.8
  occdist_scale: 0.02

  holonomic_robot: false

Costmapcommonparams:

obstacle_range: 0.5
raytrace_range: 0.5
footprint: [[-0.14, -0.14], [-0.14, 0.14], [0.14, 0.14], [0.14, -0.14]]
map_topic: /map
subscribe_to_updates: true
global_frame: map
robot_base_frame: base_link
update_frequency: 15
publish_frequency: 15
rolling_window: false

plugins:
  - {name: static_layer, type: "costmap_2d::StaticLayer"}
  - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

static_layer:
  map_topic: /map
  subscribe_to_updates: false

obstacle_layer:
    observation_sources: laser_scan_sensor
    laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

inflation_layer:
  inflation_radius: 0.2

Globalcostmapparams:

global_costmap:
  global_frame: odom
  update_frequency: 15
  publish_frequency: 15
  transform_tolerance: 0.2
  resolution: 0.1

Localcostmapparams:

local_costmap:
  update_frequency: 15
  publish_frequency: 15
  transform_tolerance: 0.2
  static_map: false
  rolling_window: true
  resolution: 0.1
  inflation_radius: 0.1
  width: 1.0
  height: 1.0

  plugins:
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}

  obstacle_layer:
    observation_sources: laser_scan_sensor
    laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

my ekf_odometry_publisher node:

/*
 * Automatic Addison
 * Date: May 20, 2021
 * ROS Version: ROS 1 - Melodic
 * Website: https://automaticaddison.com
 * Publishes odometry information for use with robot_pose_ekf package.
 *   This odometry information is based on wheel encoder tick counts.
 * Subscribe: ROS node that subscribes to the following topics:
 *  right_ticks : Tick counts from the right motor encoder (std_msgs/Int16)
 *
 *  left_ticks : Tick counts from the left motor encoder  (std_msgs/Int16)
 *
 *  initial_2d : The initial position and orientation of the robot.
 *               (geometry_msgs/PoseStamped)
 *
 * Publish: This node will publish to the following topics:
 *  odom_data_euler : Position and velocity estimate. The orientation.z
 *                    variable is an Euler angle representing the yaw angle.
 *                    (nav_msgs/Odometry)
 *  odom_data_quat : Position and velocity estimate. The orientation is
 *                   in quaternion format.
 *                   (nav_msgs/Odometry)
 * Modified from Practical Robotics in C++ book (ISBN-10 : 9389423465)
 *   by Lloyd Brombach
 */

// Include various libraries
#include "ros/ros.h"
#include "std_msgs/Int16.h"
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <cmath>

// Create odometry data publishers
ros::Publisher odom_data_pub;
ros::Publisher odom_data_pub_quat;
nav_msgs::Odometry odomNew;
nav_msgs::Odometry odomOld;

// Initial pose
const double initialX = 0.0;
const double initialY = 0.0;
const double initialTheta = 0.00000000001;
const double PI = 3.141592;

// Robot physical constants
const double TICKS_PER_REVOLUTION = 620; // For reference purposes.
const double WHEEL_RADIUS = 0.033;       // Wheel radius in meters
const double WHEEL_BASE = 0.17;          // Center of left tire to center of right tire
const double TICKS_PER_METER = 3100;     // Original was 2800

// Distance both wheels have traveled
double distanceLeft = 0;
double distanceRight = 0;

// Flag to see if initial pose has been received
bool initialPoseRecieved = false;

using namespace std;

// Get initial_2d message from either Rviz clicks or a manual pose publisher
void set_initial_2d(const geometry_msgs::PoseStamped &rvizClick)
{

  odomOld.pose.pose.position.x = rvizClick.pose.position.x;
  odomOld.pose.pose.position.y = rvizClick.pose.position.y;
  odomOld.pose.pose.orientation.z = rvizClick.pose.orientation.z;
  initialPoseRecieved = true;
}

// Calculate the distance the left wheel has traveled since the last cycle
void Calc_Left(const std_msgs::Int16 &leftCount)
{

  static int lastCountL = 0;
  if (leftCount.data != 0 && lastCountL != 0)
  {

    int leftTicks = (leftCount.data - lastCountL);

    if (leftTicks > 10000)
    {
      leftTicks = 0 - (65535 - leftTicks);
    }
    else if (leftTicks < -10000)
    {
      leftTicks = 65535 - leftTicks;
    }
    else
    {
    }
    distanceLeft = leftTicks / TICKS_PER_METER;
  }
  lastCountL = leftCount.data;
}

// Calculate the distance the right wheel has traveled since the last cycle
void Calc_Right(const std_msgs::Int16 &rightCount)
{

  static int lastCountR = 0;
  if (rightCount.data != 0 && lastCountR != 0)
  {

    int rightTicks = rightCount.data - lastCountR;

    if (rightTicks > 10000)
    {
      distanceRight = (0 - (65535 - distanceRight)) / TICKS_PER_METER;
    }
    else if (rightTicks < -10000)
    {
      rightTicks = 65535 - rightTicks;
    }
    else
    {
    }
    distanceRight = rightTicks / TICKS_PER_METER;
  }
  lastCountR = rightCount.data;
}

// Publish a nav_msgs::Odometry message in quaternion format
void publish_quat()
{

  tf2::Quaternion q;

  q.setRPY(0, 0, odomNew.pose.pose.orientation.z);

  nav_msgs::Odometry quatOdom;
  quatOdom.header.stamp = odomNew.header.stamp;
  quatOdom.header.frame_id = "odom";
  quatOdom.child_frame_id = "base_link";
  quatOdom.pose.pose.position.x = odomNew.pose.pose.position.x;
  quatOdom.pose.pose.position.y = odomNew.pose.pose.position.y;
  quatOdom.pose.pose.position.z = odomNew.pose.pose.position.z;
  quatOdom.pose.pose.orientation.x = q.x();
  quatOdom.pose.pose.orientation.y = q.y();
  quatOdom.pose.pose.orientation.z = q.z();
  quatOdom.pose.pose.orientation.w = q.w();
  quatOdom.twist.twist.linear.x = odomNew.twist.twist.linear.x;
  quatOdom.twist.twist.linear.y = odomNew.twist.twist.linear.y;
  quatOdom.twist.twist.linear.z = odomNew.twist.twist.linear.z;
  quatOdom.twist.twist.angular.x = odomNew.twist.twist.angular.x;
  quatOdom.twist.twist.angular.y = odomNew.twist.twist.angular.y;
  quatOdom.twist.twist.angular.z = odomNew.twist.twist.angular.z;

  for (int i = 0; i < 36; i++)
  {
    if (i == 0 || i == 7 || i == 14)
    {
      quatOdom.pose.covariance[i] = .01;
    }
    else if (i == 21 || i == 28 || i == 35)
    {
      quatOdom.pose.covariance[i] += 0.1;
    }
    else
    {
      quatOdom.pose.covariance[i] = 0;
    }
  }

  odom_data_pub_quat.publish(quatOdom);
}

// Update odometry information
void update_odom()
{

  // Calculate the average distance
  double cycleDistance = (distanceRight + distanceLeft) / 2;

  // Calculate the number of radians the robot has turned since the last cycle
  double cycleAngle = asin((distanceRight - distanceLeft) / WHEEL_BASE);

  // Average angle during the last cycle
  double avgAngle = cycleAngle / 2 + odomOld.pose.pose.orientation.z;

  if (avgAngle > PI)
  {
    avgAngle -= 2 * PI;
  }
  else if (avgAngle < -PI)
  {
    avgAngle += 2 * PI;
  }
  else
  {
  }

  // Calculate the new pose (x, y, and theta)
  odomNew.pose.pose.position.x = odomOld.pose.pose.position.x + cos(avgAngle) * cycleDistance;
  odomNew.pose.pose.position.y = odomOld.pose.pose.position.y + sin(avgAngle) * cycleDistance;
  odomNew.pose.pose.orientation.z = cycleAngle + odomOld.pose.pose.orientation.z;

  // Prevent lockup from a single bad cycle
  if (isnan(odomNew.pose.pose.position.x) || isnan(odomNew.pose.pose.position.y) || isnan(odomNew.pose.pose.position.z))
  {
    odomNew.pose.pose.position.x = odomOld.pose.pose.position.x;
    odomNew.pose.pose.position.y = odomOld.pose.pose.position.y;
    odomNew.pose.pose.orientation.z = odomOld.pose.pose.orientation.z;
  }

  // Make sure theta stays in the correct range
  if (odomNew.pose.pose.orientation.z > PI)
  {
    odomNew.pose.pose.orientation.z -= 2 * PI;
  }
  else if (odomNew.pose.pose.orientation.z < -PI)
  {
    odomNew.pose.pose.orientation.z += 2 * PI;
  }
  else
  {
  }

  // Compute the velocity
  odomNew.header.stamp = ros::Time::now();
  odomNew.twist.twist.linear.x = cycleDistance / (odomNew.header.stamp.toSec() - odomOld.header.stamp.toSec());
  odomNew.twist.twist.angular.z = cycleAngle / (odomNew.header.stamp.toSec() - odomOld.header.stamp.toSec());

  // Save the pose data for the next cycle
  odomOld.pose.pose.position.x = odomNew.pose.pose.position.x;
  odomOld.pose.pose.position.y = odomNew.pose.pose.position.y;
  odomOld.pose.pose.orientation.z = odomNew.pose.pose.orientation.z;
  odomOld.header.stamp = odomNew.header.stamp;

  // Publish the odometry message
  odom_data_pub.publish(odomNew);
}

int main(int argc, char **argv)
{

  // Set the data fields of the odometry message
  odomNew.header.frame_id = "odom";
  odomNew.pose.pose.position.z = 0;
  odomNew.pose.pose.orientation.x = 0;
  odomNew.pose.pose.orientation.y = 0;
  odomNew.twist.twist.linear.x = 0;
  odomNew.twist.twist.linear.y = 0;
  odomNew.twist.twist.linear.z = 0;
  odomNew.twist.twist.angular.x = 0;
  odomNew.twist.twist.angular.y = 0;
  odomNew.twist.twist.angular.z = 0;
  odomOld.pose.pose.position.x = initialX;
  odomOld.pose.pose.position.y = initialY;
  odomOld.pose.pose.orientation.z = initialTheta;

  // Launch ROS and create a node
  ros::init(argc, argv, "ekf_odom_publisher");
  ros::NodeHandle node;

  // Subscribe to ROS topics
  ros::Subscriber subForRightCounts = node.subscribe("right_ticks", 100, Calc_Right, ros::TransportHints().tcpNoDelay());
  ros::Subscriber subForLeftCounts = node.subscribe("left_ticks", 100, Calc_Left, ros::TransportHints().tcpNoDelay());
  ros::Subscriber subInitialPose = node.subscribe("initial_2d", 1, set_initial_2d);

  // Publisher of simple odom message where orientation.z is an euler angle
  odom_data_pub = node.advertise<nav_msgs::Odometry>("odom_data_euler", 100);

  // Publisher of full odom message where orientation is quaternion
  odom_data_pub_quat = node.advertise<nav_msgs::Odometry>("odom_data_quat", 100);

  ros::Rate loop_rate(30);

  while (ros::ok())
  {

    if (initialPoseRecieved)
    {
      ROS_INFO(" initial pose received yas");
      update_odom();
      publish_quat();
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}