Arduino Publisher publish() is sometimes very slow.

edit

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