Issue with Rosserial

[nishanth-rajkumar]

Hi guys, I am trying to publish tf and odom with teensy 4.1 from my robot. I am getting an error from rosserial_python node saying the following. However if I publish tf alone it's working perfectly fine.

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

I am posting the code below:

include

include

include

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 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;

} }