0
$\begingroup$

Rosanswers logo

Dear all,

I am currently running an Arduino Uno* with an ATWINC1500 WiFi breakout from Adafruit and can successfully publish topics over WiFi to a master using rosrun rosserial_python serial_node.py tcp. This has been achieved using code modified from this previous ROS answer supplied by @ahendrix.

The trouble I run into is running two of these devices at the same time. They both have unique IP addresses and are connected to the network, running differently named topics, and individually can be successfully connected and publish topics.

The first device can be connected using rosrun rosserial_python serial_node.py tcp. Simply starting a second device does not lead to be being connected. If I run a second serial_node.py with a different node name to avoid conflicts (by using rosrun rosserial_python serial_node.py tcp __name:=OtherNodeName), I get an error "socket.error: [Errno 98] Address already in use".

As the socket can't be given as an argument to serial_node.py, I tried using rosrun rosserial_server socket_node as rosserial_server in theory can handle multiple connections. Using default port 11411, I find the node displays:

Listening for rosserial TCP connection on port 11411 but never finds any devices, unlike rosserial_python serial_node.py which was successful.

Am I missing something for rosserial_server to be able to identify and connect to these devices?

Is there another way of connecting multiple TCP devices such as these arduinos with wifi simultaneously?

Thank you for your time,

Andy


UPDATE:

Having followed some very good advice from gvdhoorn, I investigated the packet traffic using Wireshark. Turns out for rosserial_server socket_node the connection between the arduino and the master must not complete (there are a few messages back and forth, but finishes before the "normal" regular published traffic commences as seen when using rosserial_python serial_node.py tcp).

By adding a short delay after nh.advertise(), the master and the arduino are able to make a connection (though socket_node reports some errors to begin with), and start publishing. This also allows for multiple arduinos to be connected! However, the connection then drops out after a minute or so from what I observe as TCP out-of-order issues.

I thought this might be because of the delay(1000) in the main loop, so I replaced this instead for using millis() and letting nh.spinonce() not get held up. This didn't improve the situation, it in fact might have made the drop out occur earlier, but I haven't done any actual timings to confirm that.

There are occurrences of TCP out-of-order when using serial_node.py, but they seem to resolve themselves very quickly and not terminate the connection between the arduino and the master when using identical code. I left this running for a few hours with no termination of connection.

Better questions:

What is it about socket_node that leads to traffic being treated differently compared to serial_node.py and lead to the connection being terminated?

What could be done to improve the stability of TCP connections when using rosserial_server socket_node, in particular when using arduinos?

Is there a more elegant way of automatically reestablishing a connection if it is lost, beyond using if (!nh.connected()){reconnect_function} ?

Thanks.


Below is the original code on the arduino for anyone who is interested:

#include <SPI.h>
#include <WiFi101.h>
#include <ros.h>
#include <std_msgs/String.h>

//////////////////////
// WiFi Definitions //
//////////////////////
#include "arduino_secrets.h"
const char* ssid[] = SECRET_SSID;
const char* password[] = SECRET_PASS;

IPAddress server(192, 168, 0, 100); // ip of your ROS server
IPAddress ip;  //Storage local IP address
int status = WL_IDLE_STATUS;

WiFiClient client;

class WiFiHardware {

  public:
  WiFiHardware() {};

  void init() {
    // do your initialization here. this probably includes TCP server/client setup
    client.connect(server, 11411);
  }

  // read a byte from the serial port. -1 = failure
  int read() {
    // implement this method so that it reads a byte from the TCP connection and returns it
    //  you may return -1 is there is an error; for example if the TCP connection is not open
    return client.read();         //will return -1 when it will works
  }

  // write data to the connection to ROS
  void write(uint8_t* data, int length) {
    // implement this so that it takes the arguments and writes or prints them to the TCP connection
    for(int i=0; i<length; i++)
      client.write(data[i]);
  }

  // returns milliseconds since start of program
  unsigned long time() {
     return millis(); // easy; did this one for you
  }
};


//ROS applicable

ros::NodeHandle_<WiFiHardware> nh;
std_msgs::String msg;
ros::Publisher string("outString", &msg);

char hello[13] = "Hello World!";


void setupWiFi()
{
  WiFi.begin(ssid, password);
  //Print to serial to find out IP address and debugging
  Serial.print("\nConnecting to "); Serial.println(ssid);
  uint8_t i = 0;
  while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
  if(i == 21){
    Serial.print("Could not connect to"); Serial.println(ssid);
    while(1) delay(500);
  }
  Serial.print("Ready! Use ");
  ip = WiFi.localIP();
  Serial.print(ip);
  Serial.println(" to access client");
}

void setup() {
  
  //Configure pins for adafruit ATWINC1500 breakout
  WiFi.setPins(8,7,4);

  
  Serial.begin(9600);
  setupWiFi();
  delay(2000);

  
  nh.initNode();
  nh.advertise(string);
}

void loop() {


  msg.data = hello;
  string.publish(&msg);
  nh.spinOnce();
  delay(1000);

  
}

*Actually a Teensy 3.2 because the Uno does not have enough memory, however it emulates and behaves like an Uno


Originally posted by Andy West on ROS Answers with karma: 81 on 2017-10-26

Post score: 2


Original comments

Comment by gvdhoorn on 2017-10-26:
I'm not a rosserial expert, but just to get a feeling for what is going on at the network level, I'd run Wireshark while trying to get things to connect. That should at least tell you whether your clients are trying to connect, how, where and in what way. It should also show server activity.

Comment by gvdhoorn on 2017-10-26:
Note that using TCP/IP over a wireless link is not a very good choice for anything interactive. Whenever I used rosserial over wireless I've used the UDP variant. Trouble is though that it currently lacks some of the functionality that the TCP implementation does have.

Comment by Andy West on 2017-10-26:
I've investigated using Wireshark. For rosserial_python serial_node.py it appears to run nicely, however for rosserial_server socket_node there are some errors being thrown around. By adding delay(100); just after nh.advertise(); the arduinos manage to connect! But dropout randomly later.

Comment by ahendrix on 2017-10-27:
The "socket.error: [Errno 98] Address already in use" suggest that both copies of serial_node.py are trying to listen on the same port number. I'm not sure if a single node is designed to handle multiple clients or not, you might want to look for documentation about this.

Comment by ahendrix on 2017-10-27:
If a single node is supposed to handle multiple clients, then this behavior is a bug. Otherwise you may want to try running the second node and its Arduino on a different port number.

Comment by ahendrix on 2017-10-27:
(Failing to handle multiple clients is still probably a bug for most software that has a listening socket, but if the docs don't say it's supported you may have a hard time getting this "bug" fixed unless you're willing to submit the fix yourself)

Comment by subarashi on 2020-09-14:
Hello dear community. I need your help. I am facing problems with rosserial_server socket_node. can you please take a look to my post? Thank you so much RosSerial_Server

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

The result of this investigation is that it may have been an intermittent bug. Having reinstalled rosserial_server just to be pedantic in checking everything, the continual dropping out of devices using rosserial_server socket.launch has be greatly reduced (drop out ~ 1 per hour compared to ~1 per minute).

The rosserial_server socket_node will accept multiple clients successfully as it should (unlike serial_node.py as ahendrix points out).

The only advice I can offer to others is to include a reconnect function as mentioned in the question. As rosserial_server is in development, if I can track down where this bug might of occurred, I will report it.

Thank you all for your help.


Originally posted by Andy West with karma: 81 on 2017-10-31

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by prnthp on 2017-11-05:
I don't know how much this applies to your problem but I use multiple rosserial_server nodes instead (one for each client) by adding args="_port:=11412", etc. to the .launch file.

Comment by Andy West on 2017-11-05:
That's a useful tip! Never though of setting separate ports to allow for separate nodes when using rosserial_server. Thanks prnthp.

Comment by ELLEO on 2018-02-22:
can you guys elaborate how to do this port modification? can i make the port number arguments changable and connect to several ports like that? please guide

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.