Problem publishing Odometry (Teensy 4.1)

edit

Hello, I am trying to publish odom topic via nav_msgs from Teensy 4.1 and the attached code can be found below. I am getting an error when I try to run rosserial_python 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 ...