My robot starts to behave weird and doing meaningless rotations during autonomous navigation (local planner or odom problem I guess)

edit

I have assembled a simple mobile robot to accomplish autonomous navigation using the navigation stack. I am using Jetson Nano (Ubuntu 18.04 & ROS Melodic), YD Lidar, and encoded dc motors. Basically following this documentation: https://www.youtube.com/watch?v=Uz_i_...

I have 2 Arduino Uno running my motor driver and communicating with Jetson Nano through two I2C buses to receive PID setpoint from ROS and to send the change in degrees of the motors in degrees back to ROS (supposed to take that change and compute odometry within ROS). Here is the code uploaded to each Arduino:

#include <PinChangeInt.h>
#include <Wire.h>
#include <PID_v1.h>
#define encodPinA1      2                       // Quadrature encoder A pin
#define encodPinB1      3                       // Quadrature encoder B pin
#define M1              5                       // PWM outputs to L298N H-Bridge motor driver module
#define dir             4

double kp =1, ki =20 , kd =0;             // modify for optimal performance
double input = 0, output = 0, setpoint = 0;
unsigned long lastTime,now;
volatile long encoderPos = 0,last_pos=0,lastpos=0;
PID myPID(&input, &output, &setpoint, kp, ki, kd,DIRECT);  
void setup() {
  pinMode(encodPinA1, INPUT_PULLUP);                  // quadrature encoder input A
  pinMode(encodPinB1, INPUT_PULLUP);                  // quadrature encoder input B
  pinMode(M1, OUTPUT);
  pinMode(dir, OUTPUT);
  attachInterrupt(0, ai1, FALLING);               // update encoder position

  TCCR1B = TCCR1B & 0b11111000 | 1;                   // To prevent Motor Noise

  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(1);
  myPID.SetOutputLimits(-255, 255);

  Wire.begin(9);                // join i2c bus with address #9 for Right Motor
  Wire.onRequest(requestEvent); // register events
  Wire.onReceive(receiveEvent);
  Serial.begin (9600); 

}

void loop() {
   now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=500 )
   {
      input = (-360.0*1000*(encoderPos-last_pos)) /(1856.0*(now - lastTime));
      lastTime=now;
      last_pos=encoderPos;
   }

  myPID.Compute();                                    // calculate new output
  pwmOut(output, setpoint);                                     // drive L298N H-Bridge module
  delay(10);
}

void pwmOut(int out, int set) {                                // to H-Bridge board
  if ((out > 0) && (set > 0)) {
    //myPID.SetOutputLimits(200, 255);
    digitalWrite(dir, LOW);
   // analogWrite(M1, 0);                             // drive motor CW
    analogWrite(M1, out);
  }
  else if ((out < 0) && (set < 0)) {
    //myPID.SetOutputLimits(-200, -255);
    digitalWrite(dir, HIGH);
    analogWrite(M1, abs(out));
  }
  else {
    analogWrite(M1, 0);  
    output = 0;
  }
}

void ai1() {
  // ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
  // Check with pin 2 to determine the direction
  if(digitalRead(encodPinB1)==HIGH) {
  encoderPos++;
  }else{
  encoderPos--;
  }
}

void requestEvent() {
  int8_t s;

  s= (-360.0*(encoderPos-lastpos))/1856.0; //change in position in degrees of the wheel
  lastpos=encoderPos;
  Wire.write(s); // respond with message of 6 bytes
  //Serial.println(s);
}

// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany)
{
  uint8_t a,b;
  a = Wire.read();
  b = Wire.read();
  setpoint= (double)((b<<8)|a);
  Serial.println((uint8_t)a);
  Serial.println((uint8_t)b);

  //Serial.println(setpoint);
}

I have a doubt in the Arduino code in the request event fn where did that 1856.0 come from, I think it is related to the encoder PPR. My hardware interface node along with launch files and config files are given below:

#include <mobile_robot_autonomous_navigation/robot_hardware_interface.h>

//namesapce ...