Arduino Publisher publish() is sometimes very slow.

asked 2018-02-12 15:56:41 -0600

TRyKKs gravatar image

Hi!

I'm currently trying to use a NodeMCU ESP8266-12E and a GY-521 breakout board (MPU6050 IMU) to send data to my computer via WiFi. I've set up the connection via

rosrun rosserial_python serial_node tcp

It connects just fine and sends data at around 400Hz, which is fine. But sometimes the the publish() method in my code (see below) takes upwards of 2 seconds to complete. I've made sure that there's always data to transmit and all that.

Is this due to it never timing out and requiring that someone is actually receiving the message? (tcp) or am I doing something weird. Any help is appreciated.

I'm using the ros_lib library aswell as the ESP8266 library for Arduino.

#include <ESP8266WiFi.h>
#include <ros.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <Wire.h>

#define LED D0
#define BRIGHT    250     //max led intensity (1-500)
#define INHALE    750    //Inhalation time in milliseconds.
#define PULSE     INHALE*1000/BRIGHT

// Select SDA and SCL pins for I2C communication
const uint8_t scl = D1;
const uint8_t sda = D2;


// Sensitivity scaling according to documentation
const uint16_t AccelScaleFactor = 16384;
const uint16_t GyroScaleFactor = 131;

//Calibration ting
float offsetAccelX = 0;
float offsetAccelY = 0;
float offsetGyroZ = 0;

// MPU6050 Slave Device Address
const uint8_t MPU6050SlaveAddress = 0x68;

// MPU6050 few configuration register addresses
const uint8_t MPU6050_REGISTER_SMPLRT_DIV   =  0x19;
const uint8_t MPU6050_REGISTER_USER_CTRL    =  0x6A;
const uint8_t MPU6050_REGISTER_PWR_MGMT_1   =  0x6B;
const uint8_t MPU6050_REGISTER_PWR_MGMT_2   =  0x6C;
const uint8_t MPU6050_REGISTER_CONFIG       =  0x1A;
const uint8_t MPU6050_REGISTER_GYRO_CONFIG  =  0x1B;
const uint8_t MPU6050_REGISTER_ACCEL_CONFIG =  0x1C;
const uint8_t MPU6050_REGISTER_FIFO_EN      =  0x23;
const uint8_t MPU6050_REGISTER_INT_ENABLE   =  0x38;
const uint8_t MPU6050_REGISTER_ACCEL_XOUT_H =  0x3B;
const uint8_t MPU6050_REGISTER_SIGNAL_PATH_RESET  = 0x68;

int16_t AccelX, AccelY, AccelZ, Temperature, GyroX, GyroY, GyroZ;



const char* ssid     = "RobustRC";
const char* password = "superspeedveryfastwow";

bool runOnce = true;

// Set the rosserial socket server IP address (Simply the IP of the host computer running roscore)
IPAddress server(192, 168, 1, 11);

// Set the rosserial socket server port (Standard communication port for rosserial)
const uint16_t serverPort = 11411;

ros::NodeHandle nh;
geometry_msgs::Vector3Stamped vec3_msg;
ros::Publisher imuPublisher("imuData0", &vec3_msg);


void setup()
{
  pinMode(LED, OUTPUT);   // LED pin as output.
  //Serial.begin(115200);
  Wire.begin();

  MPU6050_Init();
  calibrateMPU6050();
  // Connect the ESP8266 the the wifi AP using the information provided above.
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    for (int i = 1; i < BRIGHT; i++) {
      digitalWrite(LED, LOW);          // turn the LED on.
      delayMicroseconds(i * 10);       // wait
      digitalWrite(LED, HIGH);         // turn the LED off.
      delayMicroseconds(PULSE - i * 10); // wait
      delay(0);                        //to prevent watchdog firing.
    }
    //ramp decreasing intensity, Exhalation (half time):
    for (int i = BRIGHT - 1; i > 0; i--) {
      digitalWrite(LED, LOW);          // turn the LED on.
      delayMicroseconds(i * 10);        // wait
      digitalWrite(LED, HIGH);         // turn the LED off.
      delayMicroseconds(PULSE - i * 10); // wait
      i--;
      delay(0);                        //to prevent watchdog firing.
    }
  }
  // Set the connection to rosserial socket server and start the node.
  nh.getHardware()->setConnection(server, serverPort);
  nh.initNode();

  // Start publisher
  nh.advertise(imuPublisher);
}

void loop()
{

  if (!nh.connected())
  {
    digitalWrite(LED, LOW);
  }

  Read_RawValue(MPU6050SlaveAddress, MPU6050_REGISTER_ACCEL_XOUT_H);

  vec3_msg.vector.x = (float)AccelX / AccelScaleFactor  - offsetAccelX;     // Accelerometer x
  vec3_msg.vector.y = (float)AccelY / AccelScaleFactor - offsetAccelY; // Accelerometer y
  vec3_msg.vector.z = (float)GyroZ / GyroScaleFactor - offsetGyroZ;  // Yaw rate
  vec3_msg.header ...
(more)
edit retag flag offensive close merge delete

Comments

How are you measuring the time in the publish() call? It looks like you're using the LED as an output during the loop, but both the MPU6050 I2C transaction and the publish() are in that section.

ahendrix gravatar imageahendrix ( 2018-02-12 16:23:13 -0600 )edit

I don't measure time in the method itself, I simply use something like

timeThen = millis();
publish();
print( millis() - timeThen);
TRyKKs gravatar imageTRyKKs ( 2018-02-12 16:37:58 -0600 )edit

Using TCP over wireless for datastreams with high rates of small msgs is typically not the best idea.

A stall of 2 sec could point to your wireless connection being renegotiated (ie: after a drop of signal), causing TCP to buffer everything, which probably leads to your publish(..) call blocking.

gvdhoorn gravatar imagegvdhoorn ( 2018-02-13 02:39:35 -0600 )edit

Whenever I use wireless with rosserial, I switch to UDP. You'll probably lose a few messages, but it's much better suited for use over lossy links.

gvdhoorn gravatar imagegvdhoorn ( 2018-02-13 02:40:13 -0600 )edit

Ah, figured as much. Thank you! Are there any tutorials on that to point me in the right direction? Still kind of new to this.

TRyKKs gravatar imageTRyKKs ( 2018-02-13 09:50:48 -0600 )edit

Note: my comment was not meant as an authoritative answer, it was just a hypothesis.

Look at using the rosserial_udp_socket_node on the server side. Not sure any more about the client side though. I don't use Arduinos.

gvdhoorn gravatar imagegvdhoorn ( 2018-02-13 09:57:51 -0600 )edit