Rosserial thru RS485
Hello,
I am working on a project that has 3 embedded boards for different tasks linked to a PC thru a RS485 bus. We are using RS485 for a lot of reasons, especially because we have limited wires passing thru a small slipring on a joint of the robot.
So, I am trying to run rosserial on an Arduino board thru RS-485 half duplex bus. For this, I'm creating new class, inheriting an ArduinoHardware class and modifying the methods init(), read() and write() to implement the RS485 flow control. In my RS485 tranceptor circuit the pins DE e /RE are tied to a digital output pin of the microcontroller. I named this direction control pin as RTS in the code.
#define RTS 4
class NewHardware : public ArduinoHardware
{
public:
NewHardware():ArduinoHardware(&Serial1, 57600){};
void init(){
pinMode(RTS, OUTPUT);
digitalWrite(RTS, LOW);
iostream->begin(baud_);
}
int read(){
return iostream->read();
};
void write(uint8_t* data, int length){
digitalWrite(RTS, HIGH);
for(int i=0; i<length; i++){
iostream->write(data[i]);
}
while(!(UCSR1A & (1<<TXC1)));
digitalWrite(RTS, LOW);
}
};
ros::NodeHandle_<NewHardware> nh;
It works fine for short messages and low rate publishing. When I increase the size of the messages transmitted or the publishing rate the rosnode.py gives messages of Checksum error or Lost sync with device. For instance, if I run the Adc example, from the roslib, that has a big message being published at high rate i get checksum error and a lot of messages about incomplete data. If I put a delay() of 500ms or more at the end of loop() function everything works fine. I also tried the ServiceServer example and nothing works even with a huge delay, but if I comment the publisher part of the the original code the service works fine, but if I try a service response a little bigger the same problems happen.
Here a sample of error messages:
bl@BL-SENAI-PC:~$ rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=57600
[INFO] [WallTime: 1493993953.096341] ROS Serial Python Node
[INFO] [WallTime: 1493993953.100821] Connecting to /dev/ttyUSB0 at 57600 baud
[INFO] [WallTime: 1493993955.215367] wrong checksum for msg length, length -248
[INFO] [WallTime: 1493993955.216231] chk is 247
[INFO] [WallTime: 1493993955.241712] wrong checksum for topic id and msg
[ERROR] [WallTime: 1493993957.720129] Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client
[INFO] [WallTime: 1493993957.721026] Protocol version of client is Rev 0 (rosserial 0.4 and earlier), expected Rev 1 (rosserial 0.5+)
[ERROR] [WallTime: 1493993958.666128] Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client
[INFO] [WallTime: 1493993958.667115] Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+)
[INFO] [WallTime: 1493993962.715685] wrong checksum for msg length, length -1
[INFO] [WallTime: 1493993962.716585] chk is 255
[ERROR] [WallTime: 1493993968.733170] Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client
[INFO] [WallTime: 1493993968.733825] Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+)
[ERROR] [WallTime: 1493993970.206187] Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino
[ERROR] [WallTime: 1493993970.221494] Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client
[INFO] [WallTime: 1493993970.222162] Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+)
[INFO] [WallTime: 1493993970.242224] wrong checksum for topic id and msg
Another strange thing happens. I have two USB-RS485 adapters, one with a FTDI chip and another with a CH341 chip. Everything works better with the FTDI adapter. With the CH341 adapater, nothing works with the Rosserial library. I have tested both with the Auto485 libray, https://github.com/madleech/Auto485, and everything works fine with both adapters no matter the data frame sizes or bps rate.
Why these happen? What can I do to solve this problem?
Thank you in advance.
Asked by Jefecito on 2017-05-05 13:49:57 UTC
Answers
You have a single-duplex link, but I don't see any logic in your NewHardware class that prevents it from trying to transmit and receive at the same time. Obviously if it tries to do both, the received data will be lost or garbled.
You probably need to add similar single-duplex logic in the rosserial_python serial_node.py so that it doesn't attempt to transmit and receive at the same time.
(Note that the rosserial protocol will send some message on its own, so simply controlling when your code sends and receives messages is not enough)
Asked by ahendrix on 2017-05-05 23:35:52 UTC
Comments