wrong checksum for topic id and msg
Hello All, Would you please help me, I am trying to publish many topics using Teensy 3.6. I am running the comment below:
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 __name:=node1 _baud:=115200
I get the error below:
[INFO] [WallTime: 1522103896.287668] ROS Serial Python Node
[INFO] [WallTime: 1522103896.295912] Connecting to /dev/ttyACM0 at 115200 baud
[INFO] [WallTime: 1522103898.568405] Note: publish buffer size is 512 bytes
[INFO] [WallTime: 1522103898.607305] Setup publisher on /Force [std_msgs/Float32].........
[INFO] [WallTime: 1522103898.612464] Note: subscribe buffer size is 512 bytes
[INFO] [WallTime: 1522103898.637525] Setup subscriber on /th_pac1 [std_msgs/Float32].........
[INFO] [WallTime: 1522103899.149705] wrong checksum for topic id and msg
[ERROR] [WallTime: 1522103899.722798] Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client
[INFO] [WallTime: 1522103899.723073] Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+)
[INFO] [WallTime: 1522103900.296360] wrong checksum for topic id and msg
[INFO] [WallTime: 1522103901.439353] wrong checksum for topic id and msg
[INFO] [WallTime: 1522103902.013452] wrong checksum for topic id and msg
The Arduino Code is as follow:
#include <MovingAverage.h>
#include <ros.h>
#include <std_msgs/Float32.h>
//Declearing the Variables
int Valve1 = 8; int Valve2 = 9; int Valve3 = 10;
int Pump1 = 3; int Pump2 = 5; int Pump3 = 6; //PWM
int Buzz1 = 11; int Buzz2 = 12; int Buzz3 = 13; //PWM
float ff_Pdc; float ff_Pac1, lf_Pdc; float lf_Pac1, th_Pdc; float th_Pac1;
float v1, v2, v3,y,x1,x2, gram,z1, z2, z3,p1, p2, p3;
float Press_Volt1, Press_Volt2, Press_Volt3;
float e1, e2, e3;
float psi1, psi2, psi3;
float Kpa1, Kpa2, Kpa3;
float Kpa1_Ave, Kpa2_Ave, Kpa3_Ave;
ros::NodeHandle nh;
//Publish Errors
std_msgs::Float32 Error1_msg;
ros::Publisher pub_Error1("/Error1", &Error1_msg);
std_msgs::Float32 Error2_msg;
ros::Publisher pub_Error2("/Error2", &Error2_msg);
std_msgs::Float32 Error3_msg;
ros::Publisher pub_Error3("/Error3", &Error3_msg);
// Pressure Sensor Float32 Msg
std_msgs::Float32 Pressure1_msg;
ros::Publisher pub_Pressure1("/Pressure1", &Pressure1_msg);
std_msgs::Float32 Pressure2_msg;
ros::Publisher pub_Pressure2("/Pressure2", &Pressure2_msg);
std_msgs::Float32 Pressure3_msg;
ros::Publisher pub_Pressure3("/Pressure3", &Pressure3_msg);
// Pdc_Ave Sensor Float32 Msg
std_msgs::Float32 ffPdcArd_msg;
ros::Publisher pub_ffPdcArd("/ffPdcArd", &ffPdcArd_msg);
std_msgs::Float32 lfPdcArd_msg;
ros::Publisher pub_lfPdcArd("/lfPdcArd", &lfPdcArd_msg);
std_msgs::Float32 thPdcArd_msg;
ros::Publisher pub_thPdcArd("/thPdcArd", &thPdcArd_msg);
//Publish Pump Volt
std_msgs::Float32 Force_msg;
ros::Publisher pub_Force("/Force", &Force_msg);
// LoadCell Float32 Msg
std_msgs::Float32 LoadCell_msg;
ros::Publisher pub_LoadCell("/LoadCell", &LoadCell_msg);
//callback function for our subscriber to read Baxter Gripper Position
std_msgs::Float32 ff_pdc_msg;
void callbackff( const std_msgs::Float32& ff_pdc_msg){
ff_Pdc = ff_pdc_msg.data;
}
std_msgs::Float32 ff_pac1_msg;
void callbackff1( const std_msgs::Float32& ff_pac1_msg){
ff_Pac1 = ff_pac1_msg.data;
}
//////////////////////////////////////////////////////
std_msgs::Float32 lf_pdc_msg;
void callbacklf( const std_msgs::Float32& lf_pdc_msg){
lf_Pdc = lf_pdc_msg.data;
}
std_msgs::Float32 lf_pac1_msg;
void callbacklf1( const std_msgs::Float32& lf_pac1_msg){
lf_Pac1 = lf_pac1_msg.data;
}
//////////////////////////////////////////////////////
std_msgs::Float32 th_pdc_msg;
void callbackth( const std_msgs::Float32& th_pdc_msg){
th_Pdc = th_pdc_msg.data;
}
std_msgs::Float32 th_pac1_msg;
void callbackth1( const std_msgs::Float32& th_pac1_msg){
th_Pac1 = th_pac1_msg.data;
}
ros::Subscriber<std_msgs::Float32> subff("/ff_pdc", &callbackff );
ros::Subscriber<std_msgs::Float32> subff1("/ff_pac1", &callbackff1 );
ros::Subscriber<std_msgs::Float32> sublf("/lf_pdc", &callbacklf );
ros::Subscriber<std_msgs::Float32> sublf1("/lf_pac1", &callbacklf1 );
ros::Subscriber<std_msgs::Float32> subth("/th_pdc", &callbackth );
ros::Subscriber<std_msgs::Float32> subth1("/th_pac1", &callbackth1 );
// Create moving average with window size of 5.
MovingAverage average_Kpa1 (5);
MovingAverage average_Kpa2 (5);
MovingAverage average_Kpa3 (5);
void setup()
{
pinMode(Pump1, OUTPUT); pinMode(Pump2, OUTPUT); pinMode(Pump3, OUTPUT);
pinMode(Valve1, OUTPUT);pinMode(Valve2, OUTPUT);pinMode(Valve3, OUTPUT);
pinMode(Buzz1, OUTPUT); pinMode(Buzz2, OUTPUT);pinMode(Buzz3, OUTPUT);
Serial.begin(57600);
//nh.getHardware()->setBaud(115200);
nh.initNode();
nh.subscribe(subff);
nh.subscribe(subff1);
nh.subscribe(sublf);
nh.subscribe(sublf1);
nh.subscribe(subth);
nh.subscribe(subth1);
nh.advertise(pub_Error1);
nh.advertise(pub_Error2);
nh.advertise(pub_Error3);
nh.advertise(pub_Pressure1);
nh.advertise(pub_Pressure2);
nh.advertise(pub_Pressure3);
nh.advertise(pub_ffPdcArd);
nh.advertise(pub_lfPdcArd);
nh.advertise(pub_thPdcArd);
nh.advertise(pub_LoadCell);
nh.advertise(pub_Force);
}
void loop()
{
x1 = analogRead(A5); //read LoadCell
p1 = analogRead(A0); //A2 for Arduino (A9 for Teensy)//read pressure
p2 = analogRead(A1);
p3 = analogRead(A2);
Press_Volt1= p1 * (5 / 1023.0);
if (Press_Volt1 <= 0.5){
psi1 = 0;
}
else {
psi1= 3.378 * Press_Volt1 - 0.1689; // calibration equation for pressure sensor
}
Kpa1 = psi1/0.14503773800722 ;
Kpa1_Ave = average_Kpa1.update(Kpa1);
Press_Volt2= p2 * (5 / 1023.0);
if (Press_Volt2 <= 0.5){
psi2 = 0;
}
else {
psi2= 3.378 * Press_Volt2 - 0.1689; // calibration equation for pressure sensor
}
Kpa2 = psi2/0.14503773800722 ;
Kpa2_Ave = average_Kpa2.update(Kpa2);
Press_Volt3= p3 * (5 / 1023.0);
if (Press_Volt3 <= 0.5){
psi3 = 0;
}
else {
psi3=3.378 * Press_Volt3 - 0.1689; // calibration equation for pressure sensor
}
Kpa3 = psi3/0.14503773800722 ;
Kpa3_Ave = average_Kpa3.update(Kpa3);
//to calculate the voltage for LoadCell
x2 = x1 * (5/ 1023.0);
gram = 518.63 * x2; // calibration Equation for LSP2
e1 = (ff_Pdc - Kpa1_Ave);
e2 = (lf_Pdc - Kpa2_Ave);
e3 = (th_Pdc - Kpa3_Ave);
if (e1 >= 0){
z1 = map(e1, 0, 2.5, 0 , 255);
digitalWrite(Valve1, HIGH);
}
else if (e1 < 0 && e1 > -4){
digitalWrite(Valve1, HIGH);
z1=0;
}
else if(e1 <= -4) {
digitalWrite(Valve1, LOW);
z1 = 0;
}
///////////////////////////////
if (e2 >= 0){
z2 = map(e2, 0, 2.5, 0 , 255);
digitalWrite(Valve2, HIGH);
}
else if (e2 < 0 && e2 > -4){
digitalWrite(Valve2, HIGH);
z2=0;
}
else if(e2 <= -4) {
digitalWrite(Valve2, LOW);
z2 = 0;
}
//////////////////////////////////////
if (e3 >= 0){
z3 = map(e3, 0, 2.5, 0 , 255);
digitalWrite(Valve3, HIGH);
}
else if (e3 < 0 && e3 > -4){
digitalWrite(Valve3, HIGH);
z3=0;
}
else if(e3 <= -4) {
digitalWrite(Valve3, LOW);
z3 = 0;
}
analogWrite(Pump1,z1); // control the Pump through Pin 3
analogWrite(Pump2,z2);
analogWrite(Pump3,z3);
v1 = map(ff_Pac1, 0, 2000, 0, 255);
v2 = map(lf_Pac1, 0, 2000, 0, 255);
v3 = map(th_Pac1, 0, 2000, 0, 255);
analogWrite(Buzz1,v1);
analogWrite(Buzz2,v2);
analogWrite(Buzz3,v3);
/////////////////////////////////////////////////
Error1_msg.data = e1;
Error2_msg.data = e2;
Error3_msg.data = e3;
Pressure1_msg.data = Kpa1_Ave;
Pressure2_msg.data = Kpa2_Ave;
Pressure3_msg.data = Kpa3_Ave;
ffPdcArd_msg.data = ff_Pdc;
lfPdcArd_msg.data = lf_Pdc;
thPdcArd_msg.data = th_Pdc;
Force_msg.data = y;
LoadCell_msg.data = gram; //* 9.81/1000;
pub_Error1.publish( &Error1_msg);
pub_Error2.publish( &Error2_msg);
pub_Error3.publish( &Error3_msg);
pub_Pressure1.publish( &Pressure1_msg);
pub_Pressure2.publish( &Pressure2_msg);
pub_Pressure3.publish( &Pressure3_msg);
pub_ffPdcArd.publish( &ffPdcArd_msg);
pub_lfPdcArd.publish( &lfPdcArd_msg);
pub_thPdcArd.publish( &thPdcArd_msg);
pub_LoadCell.publish( &LoadCell_msg);
pub_Force.publish( &Force_msg);
nh.spinOnce();
}
I would really appreciate your help.
Thank you so much,
Best,
Moaed
Asked by MiMo on 2018-03-26 17:48:37 UTC
Answers
I had the same info and errors showing up and like in the comments suggested, the solution was to add a delay()
. In my case using delay(3)
of 3 ms solved the issue.
Note also that the delay()
won't block any interrupts (e.g using attachInterrupts
) which is described in the notes of the arduino docs:
Certain things do go on while the delay() function is controlling the Atmega chip, however, because the delay function does not disable interrupts. Serial communication that appears at the RX pin is recorded, PWM (analogWrite) values and pin states are maintained, and interrupts will work as they should.
Asked by fjp on 2020-10-13 03:09:51 UTC
Comments
This fixed a similar issue for me.
Asked by TrentonH1 on 2020-12-09 18:10:05 UTC
delay()
did help to solve the buffer overrun issue, another solution is to use millis()
to create timer for publishing messages
Asked by JC Chia on 2022-03-01 22:44:01 UTC
My solution was a bit different. I was facing the same issue and the above mentioned delay handling did not resolve it. I was using 115200 baudrate with arduino mega and it was not working. While running rosserial_arduino, I used 57600 baudrate and the "Mismatched protocol version" message was resolved, but still the "checksum" message was present. Then I realised the topic my arduino was supposed to subscribe to was not published yet. I first started the publisher node, then the rosserial with 57600 baud rate. This time there was no error message and it was working fine.
Asked by rakinrkz on 2022-09-07 07:13:15 UTC
Comments
Does the message type that you're supplying to your subscriber match that of your publisher? That's usually the message that I'll see if they don't match.
Asked by jayess on 2018-03-26 19:12:42 UTC
this could be a buffer overrun situation. How many msgs and at what rate are you trying to publish exactly?
Asked by gvdhoorn on 2018-03-27 02:42:57 UTC
jayess: the messages type (Floate32) are the same for both publishers and subscribers.
gvdhoom: I have one message for each publisher of type Floate32 since I have 11 publishers and 4 subscribers. I think it is not possible to specify the publishing rate in Arduino IDE please correct me.
Asked by MiMo on 2018-03-27 09:54:17 UTC
Publication rate is determined by how often you call
publish(..)
, which is typically inside yourloop()
function. Afaik you can influence how often that runs by usingdelay(..)
.do you really need 11 publishers? What sort of data are you publishing?
Asked by gvdhoorn on 2018-03-27 10:40:37 UTC
Perhaps you can show us some pieces of code. How is your
loop()
structured, fi?Asked by gvdhoorn on 2018-03-27 10:41:48 UTC
I don't have any delay in my loop() function in this case what is my publishing rate? Yes I need all the topics and I am publishing pressure reading from pressure sensor all my data are of type Float32.
Asked by MiMo on 2018-03-27 10:44:41 UTC
then I would suggest you add one.
Asked by gvdhoorn on 2018-03-27 10:45:52 UTC
p1 = analogRead(A0); e1 = (ff_Pdc - Kpa1_Ave); pub_Error1.publish( &Error1_msg); pub_Pressure1.publish( &Pressure1_msg); pub_ffPdcArd.publish( &ffPdcArd_msg); pub_LoadCell.publish( &LoadCell_msg);
I am subscribing to a topic from BioTac Sensor and I am controlling the pressure of soft actuator.
Asked by MiMo on 2018-03-27 10:52:06 UTC
Please edit your original question text to add the code. Use the
edit
button/link.Asked by gvdhoorn on 2018-03-27 10:52:52 UTC
I am trying to control the pressure of soft actuator and make it exactly the same as applied pressure from BioTac sensor the reason I don't have delay because I want the system to be real time.
Asked by MiMo on 2018-03-27 10:55:33 UTC
Not having a delay does not make it real-time. I don't think real-time means what you think it does.
In any case: I would suggest to add a delay just to see whether that solves your issue. Make your
loop(..)
run at say 10 Hz. If that works, then you can start solving your other problem(s).Asked by gvdhoorn on 2018-03-27 10:58:31 UTC
Have you solved the issue?
Asked by EdCherie on 2019-01-28 09:17:03 UTC