Hello, I am trying to publish odom topic via navmsgs from Teensy 4.1 and the attached code can be found below. I am getting an error when I try to run rosserialpython serial_node.py and it can be found below as well.

Error:

[ERROR] [1689103096.510274]: Tried to publish before configured, topic id 126

Code:

#include <Encoder.h>
#include <PID_v1.h>
#include <ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3.h>
#include <tf/tf.h>
#include <ros/time.h>

#define IN1 8
#define IN2 6
#define PWM1 7  //LEFT
#define IN3 9
#define IN4 11
#define PWM2 10  //RIGHT

double Pk = 50;
double Ik = 80;
double Dk = 0.3;
double Pk1 = 50;
double Ik1 = 80;
double Dk1 = 0.3;

double theta = 0;
char base_link[] = "/base_link";
char odom[] = "/odom";

float demand1, demand2, demandx, demandz, LeftOutput, RightOutput;

ros::NodeHandle nh;
geometry_msgs::TransformStamped t;
nav_msgs::Odometry odom_msg;
ros::Publisher odom_pub("odom", &odom_msg);
tf::TransformBroadcaster broadcaster;

void onTwist(const geometry_msgs::Twist& msg) {
  demandx = msg.linear.x;
  demandz = msg.angular.z;
}

ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", onTwist);

double Setpoint, Input, Output, Outputa;
PID PID1(&Input, &Output, &Setpoint, Pk, Ik, Dk, DIRECT);
double Setpoint1, Input1, Output1, Outputa1;
PID PID2(&Input1, &Output1, &Setpoint1, Pk1, Ik1, Dk1, DIRECT);



Encoder knobLeft(5, 4);
Encoder knobRight(2, 3);
double currentMillis, previousMillis, Interval;

unsigned long previousTime = 0;
const unsigned long loopInterval = 10;  // 10 milliseconds

void setup() {

  PID1.SetMode(AUTOMATIC);
  PID1.SetOutputLimits(-255, 255);
  PID1.SetSampleTime(10);
  PID2.SetMode(AUTOMATIC);
  PID2.SetOutputLimits(-255, 255);
  PID2.SetSampleTime(10);
  pinMode(IN4, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(PWM2, OUTPUT);
  pinMode(PWM1, OUTPUT);
  nh.initNode();

  nh.subscribe(sub);
  nh.advertise(odom_pub);
  broadcaster.init(nh);
}

long positionLeft = 0;
long positionRight = 0;
double distx = 0;
double disty = 0;
double deltaX = 0;
double deltaY = 0;
int timee = 0;
double pos_total_mm = 0;
float RPMLeftPrev = 0;
float LeftFilt = 0;

float RPMRightPrev = 0;
float RightFilt = 0;
void loop() {
  nh.spinOnce();
  currentMillis = millis();
  if (currentMillis - previousMillis > 10) {
    Interval = currentMillis - previousMillis;
    double newLeft, newRight, LeftDiff, RightDiff, LeftDiffmm, RightDiffmm, pos_average_mm_diff;

double LeftPPS, RightPPS, DistEachPulse, LinLeft, LinRight, RPMLeft, RPMRight;
newLeft = knobLeft.read();
newRight = knobRight.read();

LeftDiff = newLeft - positionLeft;
RightDiff = newRight - positionRight;

LeftPPS = LeftDiff * 1000 / Interval;
RightPPS = RightDiff * 1000 / Interval;

DistEachPulse = 0.0002;

LinLeft = LeftPPS * DistEachPulse;
LinRight = RightPPS * DistEachPulse;

RPMLeft = (LinLeft * 60) / (2 * 3.14 * 0.121);
RPMRight = (LinRight * 60) / (2 * 3.14 * 0.121);

LeftFilt = 0.93908194 * LeftFilt + 0.03045903 * RPMLeft + 0.03045903 * RPMLeftPrev;
RPMLeftPrev = RPMLeft;

RightFilt = 0.93908194 * RightFilt + 0.03045903 * RPMRight + 0.03045903 * RPMRightPrev;
RPMRightPrev = RPMRight;

positionLeft = newLeft;
positionRight = newRight;

demand1 = (demandx - (demandz * 0.364));  //0.73/2
demand2 = (demandx + (demandz * 0.364));

LeftOutput = (demand1 * 60) / (2 * 3.14 * 0.121);
RightOutput = (demand2 * 60) / (2 * 3.14 * 0.121);

Setpoint = LeftOutput;
Setpoint1 = RightOutput;

Input = LeftFilt;
Input1 = RightFilt;

PID1.Compute();
PID2.Compute();

if (Output > 0) {
  if (Output1 > 0) {
    Outputa = abs(Output);    //LEFT
    Outputa1 = abs(Output1);  //RIGHT
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(PWM1, Outputa);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(PWM2, Outputa1);
  } else if (Output1 < 0) {
    Outputa = abs(Output);    //LEFT
    Outputa1 = abs(Output1);  //RIGHT
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(PWM1, Outputa);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(PWM2, Outputa1);
  }
} else if (Output < 0) {
  if (Output1 > 0) {
    Outputa = abs(Output);    //LEFT
    Outputa1 = abs(Output1);  //RIGHT
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(PWM1, Outputa);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(PWM2, Outputa1);
  } else if (Output1 < 0) {
    Outputa = abs(Output);    //LEFT
    Outputa1 = abs(Output1);  //RIGHT
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(PWM1, Outputa);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(PWM2, Outputa1);
  }
}

LeftDiffmm = LeftDiff / 6.32;
RightDiffmm = RightDiff / 6.32;

// calc distance travelled based on average of both wheels
pos_average_mm_diff = (LeftDiffmm + RightDiffmm) / 2;  // difference in each cycle
pos_total_mm += pos_average_mm_diff;                   // calc total running total distance

// calc angle or rotation to broadcast with tf
theta += (RightDiffmm - LeftDiffmm) / (360 * 3.53);

if (theta > PI)
  theta -= TWO_PI;
if (theta < (-PI))
  theta += TWO_PI;

// calc x and y to broadcast with tf

distx += pos_average_mm_diff * sin(theta);
disty += pos_average_mm_diff * cos(theta);

geometry_msgs::TransformStamped t;
t.header.frame_id = odom;
t.child_frame_id = base_link;

t.transform.translation.x = distx / 1000;  // convert to metres
t.transform.translation.y = disty / 1000;
t.transform.translation.z = 0;

t.transform.rotation = tf::createQuaternionFromYaw(theta);
t.header.stamp = nh.now();

broadcaster.sendTransform(t);

nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = nh.now();
odom_msg.header.frame_id = odom;
odom_msg.pose.pose.position.x = distx / 1000;
odom_msg.pose.pose.position.y = disty / 1000;
odom_msg.pose.pose.position.z = 0.0;
odom_msg.pose.pose.orientation = tf::createQuaternionFromYaw(theta);

odom_msg.child_frame_id = base_link;
odom_msg.twist.twist.linear.x = ((LeftDiffmm + RightDiffmm) / 2) / 10;               // forward linear velovity
odom_msg.twist.twist.linear.y = 0.0;                                                 // robot does not move sideways
odom_msg.twist.twist.angular.z = ((LeftDiffmm - RightDiffmm) / (360 * 3.53)) * 100;  // anglular velocity

odom_pub.publish(&odom_msg);

previousMillis = currentMillis;
   }
}